From 9145fa053913de2e750242db01a23ab77c7324bb Mon Sep 17 00:00:00 2001 From: Awesomeplayer165 Date: Wed, 28 Feb 2024 22:11:39 -0800 Subject: [PATCH 1/3] add trajectory files from dynamic-auto-visionless --- .../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 insertions(+) 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 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 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 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 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/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 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 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 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/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/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/C1 To W3.1.traj create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.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 C3.1.traj create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.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 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 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 C1.1.traj create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.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 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 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 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/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/C3 To W3.1.traj create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.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 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 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 S3.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/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 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 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 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 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 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/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/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 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 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 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/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 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 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/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 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 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/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 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/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 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 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 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/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/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 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 C2.1.traj create mode 100644 src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.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 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/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 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/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 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 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/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 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 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 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/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 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 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 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/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.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 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/data.chor b/src/main/java/frc/robot/utils/dynamicauto/data.chor new file mode 100644 index 00000000..aae8a25b --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/data.chor @@ -0,0 +1,45944 @@ +{ + "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 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 new file mode 100644 index 00000000..0c5693aa --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C1.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 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 new file mode 100644 index 00000000..38a8bead --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C2.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 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 new file mode 100644 index 00000000..78068220 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C3.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 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 new file mode 100644 index 00000000..f0cd1af3 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C4.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 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 new file mode 100644 index 00000000..00f6917b --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To C5.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 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 W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj new file mode 100644 index 00000000..59066a5b --- /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": 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 W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj new file mode 100644 index 00000000..fb2e2531 --- /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": 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 new file mode 100644 index 00000000..331d33ff --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W3.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/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 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 S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj new file mode 100644 index 00000000..ce3947a6 --- /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": 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 S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj new file mode 100644 index 00000000..f95228f2 --- /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": -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 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 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 W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj new file mode 100644 index 00000000..1a381a4d --- /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": 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 W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj new file mode 100644 index 00000000..28a53b93 --- /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": 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 new file mode 100644 index 00000000..d8ed1eba --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W3.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/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 C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj new file mode 100644 index 00000000..90a593eb --- /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.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 C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj new file mode 100644 index 00000000..74357de2 --- /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": -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 C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj new file mode 100644 index 00000000..bcede3cb --- /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": 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 C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj new file mode 100644 index 00000000..3fd40c6e --- /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": 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 S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj new file mode 100644 index 00000000..6cbc4c1d --- /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": 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 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 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 S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj new file mode 100644 index 00000000..90749c58 --- /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": 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 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 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 W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj new file mode 100644 index 00000000..0428af29 --- /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": 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 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 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 C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj new file mode 100644 index 00000000..57334e24 --- /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": -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 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 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 C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj new file mode 100644 index 00000000..9c555123 --- /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": 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 S1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj new file mode 100644 index 00000000..e393ae58 --- /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": -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 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 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..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 new file mode 100644 index 00000000..93fe8a43 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W1.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 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 W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj new file mode 100644 index 00000000..2f31d71f --- /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": 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/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 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 C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj new file mode 100644 index 00000000..1bf739bf --- /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": 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 C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj new file mode 100644 index 00000000..0dd47978 --- /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": 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 C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj new file mode 100644 index 00000000..14b92c55 --- /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": -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 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 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 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 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 S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S4.traj new file mode 100644 index 00000000..e5689e4c --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/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/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 W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj new file mode 100644 index 00000000..f850aede --- /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": 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 new file mode 100644 index 00000000..086204d2 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W2.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 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 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 C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj new file mode 100644 index 00000000..8293ba75 --- /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": 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 C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj new file mode 100644 index 00000000..f16ed7e1 --- /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": 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 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 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 C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj new file mode 100644 index 00000000..17170fc9 --- /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": 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 S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj new file mode 100644 index 00000000..d9cc7c97 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/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/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 S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj new file mode 100644 index 00000000..773c6f5e --- /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": 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 W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj new file mode 100644 index 00000000..bec33f37 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/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/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 new file mode 100644 index 00000000..50d9a810 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W2.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 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 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 new file mode 100644 index 00000000..1818c169 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C1.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 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 new file mode 100644 index 00000000..51c46220 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C2.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 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 new file mode 100644 index 00000000..b656a1eb --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C3.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 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 new file mode 100644 index 00000000..0d7f85e3 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C4.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 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 C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj new file mode 100644 index 00000000..39d8ef61 --- /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": 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 W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj new file mode 100644 index 00000000..641af2c0 --- /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": 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 W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj new file mode 100644 index 00000000..bb0e9680 --- /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": 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/Center To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj new file mode 100644 index 00000000..aceaac81 --- /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": 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 C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj new file mode 100644 index 00000000..66955466 --- /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": 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 C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj new file mode 100644 index 00000000..63556f1f --- /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": 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 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 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 W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj new file mode 100644 index 00000000..462787d1 --- /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": 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 W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj new file mode 100644 index 00000000..53d96783 --- /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": 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 new file mode 100644 index 00000000..0a2ace26 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W3.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/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 C1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj new file mode 100644 index 00000000..29c2d4bc --- /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": 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 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 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 C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj new file mode 100644 index 00000000..2b663e69 --- /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": 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 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 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 W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj new file mode 100644 index 00000000..c913a793 --- /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": 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 W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj new file mode 100644 index 00000000..a4697851 --- /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": 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/S2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj new file mode 100644 index 00000000..4a59e123 --- /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": 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 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 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 C2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj new file mode 100644 index 00000000..69777c27 --- /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": -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 C3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj new file mode 100644 index 00000000..9d039264 --- /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": -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 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/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 C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj new file mode 100644 index 00000000..d933d22a --- /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": -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 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 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 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 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 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 C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj new file mode 100644 index 00000000..0bd0e944 --- /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": -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 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/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/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 new file mode 100644 index 00000000..b5a8c20b --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C1.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 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 new file mode 100644 index 00000000..56b4df9b --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C2.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 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 new file mode 100644 index 00000000..9e7360d5 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C3.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 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 C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj new file mode 100644 index 00000000..00bde21f --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/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/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 C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj new file mode 100644 index 00000000..818f5ad4 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/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/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 new file mode 100644 index 00000000..fccc0721 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W1.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 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 new file mode 100644 index 00000000..ab8f454f --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W2.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 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/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..ff6d2947 --- /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": -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 new file mode 100644 index 00000000..bfcd0c92 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C1.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 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 new file mode 100644 index 00000000..3bdf526e --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C2.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 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 new file mode 100644 index 00000000..9479c37b --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C3.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 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 new file mode 100644 index 00000000..b2b4ba4b --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C4.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 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 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 new file mode 100644 index 00000000..5360be44 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S1.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 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 new file mode 100644 index 00000000..08c314c4 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S2.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 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 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 S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj new file mode 100644 index 00000000..1ef15c59 --- /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": 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 W2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj new file mode 100644 index 00000000..a5fd0bb0 --- /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": 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/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 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 new file mode 100644 index 00000000..f6df5a01 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C1.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 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 new file mode 100644 index 00000000..7a28104f --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C2.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 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 new file mode 100644 index 00000000..11fcc4a7 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C3.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 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 new file mode 100644 index 00000000..84ac552d --- /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": 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 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 C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj new file mode 100644 index 00000000..cfa6b046 --- /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": 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 new file mode 100644 index 00000000..bd47bcab --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S1.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 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 S2.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj new file mode 100644 index 00000000..96b0e72c --- /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": 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 S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj new file mode 100644 index 00000000..7d72e43b --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/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/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 S4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj new file mode 100644 index 00000000..848b8926 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/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/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 W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj new file mode 100644 index 00000000..35bba061 --- /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": -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/W2 To W3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj new file mode 100644 index 00000000..f46e6706 --- /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": 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 new file mode 100644 index 00000000..8dba26f9 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C1.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 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 new file mode 100644 index 00000000..de7450e6 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C2.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 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 new file mode 100644 index 00000000..9c22ef7a --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C3.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 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 C4.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj new file mode 100644 index 00000000..ce59bb95 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/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/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 C5.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj new file mode 100644 index 00000000..8826d851 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/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/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 new file mode 100644 index 00000000..297f1b3f --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S1.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 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 new file mode 100644 index 00000000..253677d3 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S2.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 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 S3.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj new file mode 100644 index 00000000..4ff6b6c8 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/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/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 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 W1.traj b/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj new file mode 100644 index 00000000..087304c1 --- /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": -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 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..4345e451 --- /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": -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 7dd3643e3b7ed124fc5b0c943eac14e01e07606f Mon Sep 17 00:00:00 2001 From: Awesomeplayer165 Date: Wed, 28 Feb 2024 22:25:20 -0800 Subject: [PATCH 2/3] update trajectories --- .../data.chor => dynamic-auto.chor | 0 simgui.json | 10 +- 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 +++++++++ .../deploy/choreo/Amp Side To W1.traj | 0 .../deploy/choreo/Amp Side To W2.traj | 0 src/main/deploy/choreo/Amp Side To W3.traj | 193 +++ .../deploy/choreo/C1 To C2.traj | 0 .../deploy/choreo/C1 To C3.traj | 0 .../deploy/choreo/C1 To C4.traj | 0 .../deploy/choreo/C1 To C5.traj | 0 .../deploy/choreo/C1 To S1.traj | 0 .../deploy/choreo/C1 To S2.traj | 0 .../deploy/choreo/C1 To S3.traj | 0 .../deploy/choreo/C1 To S4.traj | 0 .../deploy/choreo/C1 To W1.traj | 0 .../deploy/choreo/C1 To W2.traj | 0 src/main/deploy/choreo/C1 To W3.traj | 274 ++++ .../deploy/choreo/C2 To C1.traj | 0 .../deploy/choreo/C2 To C3.traj | 0 .../deploy/choreo/C2 To C4.traj | 0 .../deploy/choreo/C2 To C5.traj | 0 .../deploy/choreo/C2 To S1.traj | 0 .../deploy/choreo/C2 To S2.traj | 0 .../deploy/choreo/C2 To S3.traj | 0 .../deploy/choreo/C2 To S4.traj | 0 .../deploy/choreo/C2 To W1.traj | 0 .../deploy/choreo/C2 To W2.traj | 0 .../deploy/choreo/C2 To W3.traj | 0 .../deploy/choreo/C3 To C1.traj | 0 .../deploy/choreo/C3 To C2.traj | 0 .../deploy/choreo/C3 To C4.traj | 0 .../deploy/choreo/C3 To C5.traj | 0 .../deploy/choreo/C3 To S1.traj | 0 .../deploy/choreo/C3 To S2.traj | 0 .../deploy/choreo/C3 To S3.traj | 0 .../deploy/choreo/C3 To S4.traj | 0 src/main/deploy/choreo/C3 To W1.traj | 220 +++ .../deploy/choreo/C3 To W2.traj | 0 .../deploy/choreo/C3 To W3.traj | 0 .../deploy/choreo/C4 To C1.traj | 0 .../deploy/choreo/C4 To C2.traj | 0 .../deploy/choreo/C4 To C3.traj | 0 .../deploy/choreo/C4 To C5.traj | 0 .../deploy/choreo/C4 To S1.traj | 0 .../deploy/choreo/C4 To S2.traj | 0 .../deploy/choreo/C4 To S3.traj | 0 src/main/deploy/choreo/C4 To S4.traj | 211 +++ .../deploy/choreo/C4 To W1.traj | 0 src/main/deploy/choreo/C4 To W2.traj | 256 +++ .../deploy/choreo/C4 To W3.traj | 0 .../deploy/choreo/C5 To C1.traj | 0 .../deploy/choreo/C5 To C2.traj | 0 .../deploy/choreo/C5 To C3.traj | 0 .../deploy/choreo/C5 To C4.traj | 0 .../deploy/choreo/C5 To S1.traj | 0 .../deploy/choreo/C5 To S2.traj | 0 .../deploy/choreo/C5 To S3.traj | 0 .../deploy/choreo/C5 To S4.traj | 0 .../deploy/choreo/C5 To W1.traj | 0 src/main/deploy/choreo/C5 To W2.traj | 283 ++++ .../deploy/choreo/C5 To W3.traj | 0 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 +++++++++++++ .../deploy/choreo/Center To C5.traj | 0 .../deploy/choreo/Center To W1.traj | 0 .../deploy/choreo/Center To W2.traj | 0 .../deploy/choreo/Center To W3.traj | 0 .../deploy/choreo/S1 To C1.traj | 0 .../deploy/choreo/S1 To C2.traj | 0 .../deploy/choreo/S1 To C3.traj | 0 .../deploy/choreo/S1 To C4.traj | 0 .../deploy/choreo/S1 To C5.traj | 0 .../deploy/choreo/S1 To W1.traj | 0 .../deploy/choreo/S1 To W2.traj | 0 src/main/deploy/choreo/S1 To W3.traj | 175 ++ .../deploy/choreo/S2 To C1.traj | 0 .../deploy/choreo/S2 To C2.traj | 0 .../deploy/choreo/S2 To C3.traj | 0 .../deploy/choreo/S2 To C4.traj | 0 .../deploy/choreo/S2 To C5.traj | 0 .../deploy/choreo/S2 To W1.traj | 0 .../deploy/choreo/S2 To W2.traj | 0 .../deploy/choreo/S2 To W3.traj | 0 .../deploy/choreo/S3 To C1.traj | 0 .../deploy/choreo/S3 To C2.traj | 0 .../deploy/choreo/S3 To C3.traj | 0 .../deploy/choreo/S3 To C4.traj | 0 .../deploy/choreo/S3 To C5.traj | 0 .../deploy/choreo/S3 To W1.traj | 0 .../deploy/choreo/S3 To W2.traj | 0 .../deploy/choreo/S3 To W3.traj | 0 .../deploy/choreo/S4 To C1.traj | 0 .../deploy/choreo/S4 To C2.traj | 0 .../deploy/choreo/S4 To C3.traj | 0 .../deploy/choreo/S4 To C4.traj | 0 .../deploy/choreo/S4 To C5.traj | 0 .../deploy/choreo/S4 To W1.traj | 0 .../deploy/choreo/S4 To W2.traj | 0 .../deploy/choreo/S4 To W3.traj | 0 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 ++++ .../deploy/choreo/Stage Side To C4.traj | 0 .../deploy/choreo/Stage Side To C5.traj | 0 src/main/deploy/choreo/Stage Side To W1.traj | 193 +++ src/main/deploy/choreo/Stage Side To W2.traj | 148 ++ .../deploy/choreo/Stage Side To W3.traj | 0 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 +++++ .../deploy/choreo/W1 To C5.traj | 0 src/main/deploy/choreo/W1 To S1.traj | 130 ++ src/main/deploy/choreo/W1 To S2.traj | 130 ++ .../deploy/choreo/W1 To S3.traj | 0 .../deploy/choreo/W1 To S4.traj | 0 .../deploy/choreo/W1 To W2.traj | 0 .../deploy/choreo/W1 To W3.traj | 0 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 +++++++ .../deploy/choreo/W2 To C5.traj | 0 src/main/deploy/choreo/W2 To S1.traj | 148 ++ .../deploy/choreo/W2 To S2.traj | 0 .../deploy/choreo/W2 To S3.traj | 0 .../deploy/choreo/W2 To S4.traj | 0 .../deploy/choreo/W2 To W1.traj | 0 .../deploy/choreo/W2 To W3.traj | 0 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 ++++ .../deploy/choreo/W3 To C4.traj | 0 .../deploy/choreo/W3 To C5.traj | 0 src/main/deploy/choreo/W3 To S1.traj | 175 ++ src/main/deploy/choreo/W3 To S2.traj | 130 ++ .../deploy/choreo/W3 To S3.traj | 0 .../deploy/choreo/W3 To S4.traj | 0 .../deploy/choreo/W3 To W1.traj | 0 .../deploy/choreo/W3 To W2.traj | 0 .../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 W2.1.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 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 | 346 ---- .../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 | 265 --- .../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 S2.1.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 S4.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 | 319 ---- .../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 S2.1.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 | 346 ---- .../dynamicauto/deploy/choreo/C5 To W3.1.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 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 | 283 ---- .../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 | 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 C5.1.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 -- .../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 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 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 | 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 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 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 | 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 C5.1.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 S4.1.traj | 256 --- .../dynamicauto/deploy/choreo/W3 To W1.1.traj | 148 -- .../dynamicauto/deploy/choreo/W3 To W2.1.traj | 103 -- 327 files changed, 12573 insertions(+), 53568 deletions(-) rename src/main/java/frc/robot/utils/dynamicauto/data.chor => dynamic-auto.chor (100%) 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 rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/Amp Side To W1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/Amp Side To W2.traj (100%) create mode 100644 src/main/deploy/choreo/Amp Side To W3.traj rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C1 To C2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C1 To C3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C1 To C4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C1 To C5.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C1 To S1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C1 To S2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C1 To S3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C1 To S4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C1 To W1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C1 To W2.traj (100%) create mode 100644 src/main/deploy/choreo/C1 To W3.traj rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C2 To C1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C2 To C3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C2 To C4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C2 To C5.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C2 To S1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C2 To S2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C2 To S3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C2 To S4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C2 To W1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C2 To W2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C2 To W3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C3 To C1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C3 To C2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C3 To C4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C3 To C5.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C3 To S1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C3 To S2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C3 To S3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C3 To S4.traj (100%) create mode 100644 src/main/deploy/choreo/C3 To W1.traj rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C3 To W2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C3 To W3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C4 To C1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C4 To C2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C4 To C3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C4 To C5.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C4 To S1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C4 To S2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C4 To S3.traj (100%) create mode 100644 src/main/deploy/choreo/C4 To S4.traj rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C4 To W1.traj (100%) create mode 100644 src/main/deploy/choreo/C4 To W2.traj rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C4 To W3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C5 To C1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C5 To C2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C5 To C3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C5 To C4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C5 To S1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C5 To S2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C5 To S3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C5 To S4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C5 To W1.traj (100%) create mode 100644 src/main/deploy/choreo/C5 To W2.traj rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/C5 To W3.traj (100%) 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 rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/Center To C5.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/Center To W1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/Center To W2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/Center To W3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S1 To C1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S1 To C2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S1 To C3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S1 To C4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S1 To C5.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S1 To W1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S1 To W2.traj (100%) create mode 100644 src/main/deploy/choreo/S1 To W3.traj rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S2 To C1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S2 To C2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S2 To C3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S2 To C4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S2 To C5.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S2 To W1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S2 To W2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S2 To W3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S3 To C1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S3 To C2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S3 To C3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S3 To C4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S3 To C5.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S3 To W1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S3 To W2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S3 To W3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S4 To C1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S4 To C2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S4 To C3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S4 To C4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S4 To C5.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S4 To W1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S4 To W2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/S4 To W3.traj (100%) 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 rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/Stage Side To C4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/Stage Side To C5.traj (100%) create mode 100644 src/main/deploy/choreo/Stage Side To W1.traj create mode 100644 src/main/deploy/choreo/Stage Side To W2.traj rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/Stage Side To W3.traj (100%) 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 rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W1 To C5.traj (100%) create mode 100644 src/main/deploy/choreo/W1 To S1.traj create mode 100644 src/main/deploy/choreo/W1 To S2.traj rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W1 To S3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W1 To S4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W1 To W2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W1 To W3.traj (100%) 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 rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W2 To C5.traj (100%) create mode 100644 src/main/deploy/choreo/W2 To S1.traj rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W2 To S2.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W2 To S3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W2 To S4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W2 To W1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W2 To W3.traj (100%) 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 rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W3 To C4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W3 To C5.traj (100%) create mode 100644 src/main/deploy/choreo/W3 To S1.traj create mode 100644 src/main/deploy/choreo/W3 To S2.traj rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W3 To S3.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W3 To S4.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W3 To W1.traj (100%) rename src/main/{java/frc/robot/utils/dynamicauto => }/deploy/choreo/W3 To W2.traj (100%) 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 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/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 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/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 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 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 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 S1.1.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 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 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 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/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 S1.1.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 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 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/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 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/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 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 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 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 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/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 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 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 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 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 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 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 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 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 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/data.chor b/dynamic-auto.chor similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/data.chor rename to dynamic-auto.chor diff --git a/simgui.json b/simgui.json index 2dd2f18e..01650fa7 100644 --- a/simgui.json +++ b/simgui.json @@ -11,16 +11,16 @@ "/AdvantageKit/RealOutputs/Elevator/Mechanism2d": "Mechanism2d", "/AdvantageKit/RealOutputs/Shooter/Mechanism2d": "Mechanism2d", "/FMSInfo": "FMSInfo", - "/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/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" + "/SmartDashboard/Shooter shoot": "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" } }, "NetworkTables": { 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/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj b/src/main/deploy/choreo/Amp Side To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W1.traj rename to src/main/deploy/choreo/Amp Side To W1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj b/src/main/deploy/choreo/Amp Side To W2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Amp Side To W2.traj rename to src/main/deploy/choreo/Amp Side To W2.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.traj b/src/main/deploy/choreo/C1 To C2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C2.traj rename to src/main/deploy/choreo/C1 To C2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.traj b/src/main/deploy/choreo/C1 To C3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C3.traj rename to src/main/deploy/choreo/C1 To C3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj b/src/main/deploy/choreo/C1 To C4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C4.traj rename to src/main/deploy/choreo/C1 To C4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj b/src/main/deploy/choreo/C1 To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To C5.traj rename to src/main/deploy/choreo/C1 To C5.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj b/src/main/deploy/choreo/C1 To S1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S1.traj rename to src/main/deploy/choreo/C1 To S1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj b/src/main/deploy/choreo/C1 To S2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S2.traj rename to src/main/deploy/choreo/C1 To S2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.traj b/src/main/deploy/choreo/C1 To S3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S3.traj rename to src/main/deploy/choreo/C1 To S3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.traj b/src/main/deploy/choreo/C1 To S4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To S4.traj rename to src/main/deploy/choreo/C1 To S4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj b/src/main/deploy/choreo/C1 To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W1.traj rename to src/main/deploy/choreo/C1 To W1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj b/src/main/deploy/choreo/C1 To W2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C1 To W2.traj rename to src/main/deploy/choreo/C1 To W2.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj b/src/main/deploy/choreo/C2 To C1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C1.traj rename to src/main/deploy/choreo/C2 To C1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj b/src/main/deploy/choreo/C2 To C3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C3.traj rename to src/main/deploy/choreo/C2 To C3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj b/src/main/deploy/choreo/C2 To C4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C4.traj rename to src/main/deploy/choreo/C2 To C4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj b/src/main/deploy/choreo/C2 To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To C5.traj rename to src/main/deploy/choreo/C2 To C5.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj b/src/main/deploy/choreo/C2 To S1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S1.traj rename to src/main/deploy/choreo/C2 To S1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj b/src/main/deploy/choreo/C2 To S2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S2.traj rename to src/main/deploy/choreo/C2 To S2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj b/src/main/deploy/choreo/C2 To S3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S3.traj rename to src/main/deploy/choreo/C2 To S3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.traj b/src/main/deploy/choreo/C2 To S4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To S4.traj rename to src/main/deploy/choreo/C2 To S4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj b/src/main/deploy/choreo/C2 To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W1.traj rename to src/main/deploy/choreo/C2 To W1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj b/src/main/deploy/choreo/C2 To W2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W2.traj rename to src/main/deploy/choreo/C2 To W2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj b/src/main/deploy/choreo/C2 To W3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C2 To W3.traj rename to src/main/deploy/choreo/C2 To W3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj b/src/main/deploy/choreo/C3 To C1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C1.traj rename to src/main/deploy/choreo/C3 To C1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.traj b/src/main/deploy/choreo/C3 To C2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C2.traj rename to src/main/deploy/choreo/C3 To C2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj b/src/main/deploy/choreo/C3 To C4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C4.traj rename to src/main/deploy/choreo/C3 To C4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj b/src/main/deploy/choreo/C3 To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To C5.traj rename to src/main/deploy/choreo/C3 To C5.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj b/src/main/deploy/choreo/C3 To S1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S1.traj rename to src/main/deploy/choreo/C3 To S1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.traj b/src/main/deploy/choreo/C3 To S2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S2.traj rename to src/main/deploy/choreo/C3 To S2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.traj b/src/main/deploy/choreo/C3 To S3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S3.traj rename to src/main/deploy/choreo/C3 To S3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.traj b/src/main/deploy/choreo/C3 To S4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To S4.traj rename to src/main/deploy/choreo/C3 To S4.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj b/src/main/deploy/choreo/C3 To W2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W2.traj rename to src/main/deploy/choreo/C3 To W2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.traj b/src/main/deploy/choreo/C3 To W3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C3 To W3.traj rename to src/main/deploy/choreo/C3 To W3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj b/src/main/deploy/choreo/C4 To C1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C1.traj rename to src/main/deploy/choreo/C4 To C1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj b/src/main/deploy/choreo/C4 To C2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C2.traj rename to src/main/deploy/choreo/C4 To C2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj b/src/main/deploy/choreo/C4 To C3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C3.traj rename to src/main/deploy/choreo/C4 To C3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.traj b/src/main/deploy/choreo/C4 To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To C5.traj rename to src/main/deploy/choreo/C4 To C5.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.traj b/src/main/deploy/choreo/C4 To S1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S1.traj rename to src/main/deploy/choreo/C4 To S1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.traj b/src/main/deploy/choreo/C4 To S2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S2.traj rename to src/main/deploy/choreo/C4 To S2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.traj b/src/main/deploy/choreo/C4 To S3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To S3.traj rename to src/main/deploy/choreo/C4 To S3.traj 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..1d7c5583 --- /dev/null +++ b/src/main/deploy/choreo/C4 To S4.traj @@ -0,0 +1,211 @@ +{ + "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/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj b/src/main/deploy/choreo/C4 To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W1.traj rename to src/main/deploy/choreo/C4 To W1.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.traj b/src/main/deploy/choreo/C4 To W3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C4 To W3.traj rename to src/main/deploy/choreo/C4 To W3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj b/src/main/deploy/choreo/C5 To C1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C1.traj rename to src/main/deploy/choreo/C5 To C1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj b/src/main/deploy/choreo/C5 To C2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C2.traj rename to src/main/deploy/choreo/C5 To C2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.traj b/src/main/deploy/choreo/C5 To C3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C3.traj rename to src/main/deploy/choreo/C5 To C3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj b/src/main/deploy/choreo/C5 To C4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To C4.traj rename to src/main/deploy/choreo/C5 To C4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.traj b/src/main/deploy/choreo/C5 To S1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S1.traj rename to src/main/deploy/choreo/C5 To S1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.traj b/src/main/deploy/choreo/C5 To S2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S2.traj rename to src/main/deploy/choreo/C5 To S2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj b/src/main/deploy/choreo/C5 To S3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S3.traj rename to src/main/deploy/choreo/C5 To S3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj b/src/main/deploy/choreo/C5 To S4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To S4.traj rename to src/main/deploy/choreo/C5 To S4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj b/src/main/deploy/choreo/C5 To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W1.traj rename to src/main/deploy/choreo/C5 To W1.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.traj b/src/main/deploy/choreo/C5 To W3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/C5 To W3.traj rename to src/main/deploy/choreo/C5 To W3.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj b/src/main/deploy/choreo/Center To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To C5.traj rename to src/main/deploy/choreo/Center To C5.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj b/src/main/deploy/choreo/Center To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W1.traj rename to src/main/deploy/choreo/Center To W1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj b/src/main/deploy/choreo/Center To W2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W2.traj rename to src/main/deploy/choreo/Center To W2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj b/src/main/deploy/choreo/Center To W3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Center To W3.traj rename to src/main/deploy/choreo/Center To W3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj b/src/main/deploy/choreo/S1 To C1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C1.traj rename to src/main/deploy/choreo/S1 To C1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj b/src/main/deploy/choreo/S1 To C2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C2.traj rename to src/main/deploy/choreo/S1 To C2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.traj b/src/main/deploy/choreo/S1 To C3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C3.traj rename to src/main/deploy/choreo/S1 To C3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.traj b/src/main/deploy/choreo/S1 To C4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C4.traj rename to src/main/deploy/choreo/S1 To C4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.traj b/src/main/deploy/choreo/S1 To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To C5.traj rename to src/main/deploy/choreo/S1 To C5.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj b/src/main/deploy/choreo/S1 To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W1.traj rename to src/main/deploy/choreo/S1 To W1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj b/src/main/deploy/choreo/S1 To W2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S1 To W2.traj rename to src/main/deploy/choreo/S1 To W2.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj b/src/main/deploy/choreo/S2 To C1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C1.traj rename to src/main/deploy/choreo/S2 To C1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.traj b/src/main/deploy/choreo/S2 To C2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C2.traj rename to src/main/deploy/choreo/S2 To C2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj b/src/main/deploy/choreo/S2 To C3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C3.traj rename to src/main/deploy/choreo/S2 To C3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.traj b/src/main/deploy/choreo/S2 To C4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C4.traj rename to src/main/deploy/choreo/S2 To C4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.traj b/src/main/deploy/choreo/S2 To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To C5.traj rename to src/main/deploy/choreo/S2 To C5.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj b/src/main/deploy/choreo/S2 To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W1.traj rename to src/main/deploy/choreo/S2 To W1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj b/src/main/deploy/choreo/S2 To W2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W2.traj rename to src/main/deploy/choreo/S2 To W2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj b/src/main/deploy/choreo/S2 To W3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S2 To W3.traj rename to src/main/deploy/choreo/S2 To W3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.traj b/src/main/deploy/choreo/S3 To C1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C1.traj rename to src/main/deploy/choreo/S3 To C1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj b/src/main/deploy/choreo/S3 To C2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C2.traj rename to src/main/deploy/choreo/S3 To C2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj b/src/main/deploy/choreo/S3 To C3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C3.traj rename to src/main/deploy/choreo/S3 To C3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.traj b/src/main/deploy/choreo/S3 To C4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C4.traj rename to src/main/deploy/choreo/S3 To C4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj b/src/main/deploy/choreo/S3 To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To C5.traj rename to src/main/deploy/choreo/S3 To C5.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.traj b/src/main/deploy/choreo/S3 To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W1.traj rename to src/main/deploy/choreo/S3 To W1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj b/src/main/deploy/choreo/S3 To W2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W2.traj rename to src/main/deploy/choreo/S3 To W2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj b/src/main/deploy/choreo/S3 To W3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S3 To W3.traj rename to src/main/deploy/choreo/S3 To W3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.traj b/src/main/deploy/choreo/S4 To C1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C1.traj rename to src/main/deploy/choreo/S4 To C1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.traj b/src/main/deploy/choreo/S4 To C2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C2.traj rename to src/main/deploy/choreo/S4 To C2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.traj b/src/main/deploy/choreo/S4 To C3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C3.traj rename to src/main/deploy/choreo/S4 To C3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj b/src/main/deploy/choreo/S4 To C4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C4.traj rename to src/main/deploy/choreo/S4 To C4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj b/src/main/deploy/choreo/S4 To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To C5.traj rename to src/main/deploy/choreo/S4 To C5.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.traj b/src/main/deploy/choreo/S4 To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W1.traj rename to src/main/deploy/choreo/S4 To W1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.traj b/src/main/deploy/choreo/S4 To W2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W2.traj rename to src/main/deploy/choreo/S4 To W2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj b/src/main/deploy/choreo/S4 To W3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/S4 To W3.traj rename to src/main/deploy/choreo/S4 To W3.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj b/src/main/deploy/choreo/Stage Side To C4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C4.traj rename to src/main/deploy/choreo/Stage Side To C4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj b/src/main/deploy/choreo/Stage Side To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To C5.traj rename to src/main/deploy/choreo/Stage Side To C5.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj b/src/main/deploy/choreo/Stage Side To W3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/Stage Side To W3.traj rename to src/main/deploy/choreo/Stage Side To W3.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj b/src/main/deploy/choreo/W1 To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To C5.traj rename to src/main/deploy/choreo/W1 To C5.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj b/src/main/deploy/choreo/W1 To S3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S3.traj rename to src/main/deploy/choreo/W1 To S3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj b/src/main/deploy/choreo/W1 To S4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To S4.traj rename to src/main/deploy/choreo/W1 To S4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj b/src/main/deploy/choreo/W1 To W2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W2.traj rename to src/main/deploy/choreo/W1 To W2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj b/src/main/deploy/choreo/W1 To W3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W1 To W3.traj rename to src/main/deploy/choreo/W1 To W3.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj b/src/main/deploy/choreo/W2 To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To C5.traj rename to src/main/deploy/choreo/W2 To C5.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj b/src/main/deploy/choreo/W2 To S2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S2.traj rename to src/main/deploy/choreo/W2 To S2.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj b/src/main/deploy/choreo/W2 To S3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S3.traj rename to src/main/deploy/choreo/W2 To S3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj b/src/main/deploy/choreo/W2 To S4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To S4.traj rename to src/main/deploy/choreo/W2 To S4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj b/src/main/deploy/choreo/W2 To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W1.traj rename to src/main/deploy/choreo/W2 To W1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj b/src/main/deploy/choreo/W2 To W3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W2 To W3.traj rename to src/main/deploy/choreo/W2 To W3.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj b/src/main/deploy/choreo/W3 To C4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C4.traj rename to src/main/deploy/choreo/W3 To C4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj b/src/main/deploy/choreo/W3 To C5.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To C5.traj rename to src/main/deploy/choreo/W3 To C5.traj 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/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj b/src/main/deploy/choreo/W3 To S3.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S3.traj rename to src/main/deploy/choreo/W3 To S3.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj b/src/main/deploy/choreo/W3 To S4.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To S4.traj rename to src/main/deploy/choreo/W3 To S4.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj b/src/main/deploy/choreo/W3 To W1.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W1.traj rename to src/main/deploy/choreo/W3 To W1.traj diff --git a/src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj b/src/main/deploy/choreo/W3 To W2.traj similarity index 100% rename from src/main/java/frc/robot/utils/dynamicauto/deploy/choreo/W3 To W2.traj rename to src/main/deploy/choreo/W3 To W2.traj 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 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 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 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 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 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 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 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 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 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 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 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 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/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 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 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 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 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/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 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 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 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 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 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 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/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 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 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 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 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 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 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 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 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 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 bf11ebf8f6d97927c92c6032ae4c151199d8ba62 Mon Sep 17 00:00:00 2001 From: Awesomeplayer165 Date: Wed, 28 Feb 2024 22:27:05 -0800 Subject: [PATCH 3/3] add advantage scope layout file --- .../robot/utils/dynamicauto/AutoAimSetup.json | 111 ++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 src/main/java/frc/robot/utils/dynamicauto/AutoAimSetup.json diff --git a/src/main/java/frc/robot/utils/dynamicauto/AutoAimSetup.json b/src/main/java/frc/robot/utils/dynamicauto/AutoAimSetup.json new file mode 100644 index 00000000..a5934d00 --- /dev/null +++ b/src/main/java/frc/robot/utils/dynamicauto/AutoAimSetup.json @@ -0,0 +1,111 @@ +{ + "hubs": [ + { + "x": 242, + "y": 205, + "width": 1214, + "height": 681, + "state": { + "sidebar": { + "width": 385, + "expanded": [ + "/AdvantageKit", + "/AdvantageKit/RealOutputs", + "/AdvantageKit/RealOutputs/PathPlanner", + "/AdvantageKit/RealOutputs/Elevator", + "/AdvantageKit/RealOutputs/Odometry", + "/AdvantageKit/RealOutputs/AutoAim", + "/AdvantageKit/RealOutputs/DynamicAuto" + ] + }, + "tabs": { + "selected": 1, + "tabs": [ + { + "type": 0, + "path": "../docs/INDEX.md" + }, + { + "type": 6, + "uuid": "godheywfexnuq7b5nde379abz8r3vvtq", + "fields": [], + "listFields": [ + [ + { + "type": "Component (Robot)", + "key": "NT:/AdvantageKit/RealOutputs/Elevator/Carriage Pose", + "sourceTypeIndex": 1, + "sourceType": "Pose3d" + }, + { + "type": "Robot", + "key": "NT:/PathPlanner/activePath", + "sourceTypeIndex": 0, + "sourceType": 5 + } + ], + [ + { + "type": "Robot", + "key": "NT:/AdvantageKit/RealOutputs/Odometry/Robot", + "sourceTypeIndex": 1, + "sourceType": "Pose2d" + }, + { + "type": "Green Ghost", + "key": "NT:/AdvantageKit/RealOutputs/DynamicAuto/Current Path Initial Pose", + "sourceTypeIndex": 1, + "sourceType": "Pose2d" + }, + { + "type": "Yellow Ghost", + "key": "NT:/AdvantageKit/RealOutputs/DynamicAuto/Current Path Ending Pose", + "sourceTypeIndex": 1, + "sourceType": "Pose2d" + }, + { + "type": "Trajectory", + "key": "NT:/AdvantageKit/RealOutputs/DynamicAuto/Forward Trajectory Followed", + "sourceTypeIndex": 2, + "sourceType": "Pose2d[]" + }, + { + "type": "Trajectory", + "key": "NT:/AdvantageKit/RealOutputs/DynamicAuto/Curent Trajectory Followed", + "sourceTypeIndex": 2, + "sourceType": "Pose2d[]" + } + ] + ], + "options": { + "field": "2024 Field", + "alliance": "blue", + "robot": "8033 Crescendo", + "unitDistance": "meters", + "unitRotation": "radians" + }, + "configHidden": false, + "visualizer": { + "cameraIndex": -2, + "orbitFov": 50, + "cameraPosition": [ + -8.608288762599031, + 10.771000922833943, + 7.312892116957138 + ], + "cameraTarget": [ + -6.483848096358995, + 0.8606557485222912, + 7.657088875749636 + ] + }, + "title": "3D Field" + } + ] + } + } + } + ], + "satellites": [], + "version": "3.1.0" +}