From 0969d185c5e84dac553054d6ac89eb0bad5dfe03 Mon Sep 17 00:00:00 2001
From: Myles Pasetsky <34213819+selym3@users.noreply.github.com>
Date: Mon, 28 Mar 2022 17:58:36 -0400
Subject: [PATCH] Competition changes from hofstra (#101)

* upload test limelight config

* upload hofstra limelight configuration

* upload sam's perfect little hofstra limelight configuration

* add port forwarding for limelight [SLOW]

* add updated hofstra pipeline

* extend the get ball path to the competition field

* revert shooting wait time to default

* add led for start of climb routine

* add forwarding for all limelight ports

* hardcode shuffleboard again

* bring path back by 2 inches

* Fix Match Ending LEDs

* Spotless Apply

* make color sensor less aggressive

* add led swap mode

* add led swap to shuffleboard

* shuffleboard?

* update climb time led

* add more end gmae time-based leds

* fix order of leds

* add min match time for leds and clean code

* adjust shot distance to be closer

Co-authored-by: Prog694 <prog692@gmail.com>
---
 .../Paths/FiveBallGetTerminalBalls.path       |    2 +-
 .../PathWeaver/Paths/FiveBallGetWallBall.path |    2 +-
 .../Paths/FiveBallShootTerminalBalls.path     |    2 +-
 limelights/Hofstra.vpr                        |   68 +
 limelights/test.vpr                           |   64 +
 shuffleboard.json                             |   19 +-
 .../FiveBallGetTerminalBalls.wpilib.json      |  715 ++++-----
 .../output/FiveBallGetWallBall.wpilib.json    |  112 +-
 .../FiveBallShootTerminalBalls.wpilib.json    | 1344 +++++++++--------
 src/main/java/com/stuypulse/robot/Robot.java  |    5 +-
 .../com/stuypulse/robot/RobotContainer.java   |    3 +-
 .../drivetrain/DrivetrainAlignCommand.java    |    3 +-
 .../DrivetrainAlignToShootCommand.java        |    2 +-
 .../stuypulse/robot/constants/Settings.java   |   12 +-
 .../robot/subsystems/ColorSensor.java         |    4 +-
 .../robot/subsystems/LEDController.java       |   17 +-
 .../java/com/stuypulse/robot/util/Target.java |   12 +
 17 files changed, 1304 insertions(+), 1082 deletions(-)
 create mode 100644 limelights/Hofstra.vpr
 create mode 100644 limelights/test.vpr

diff --git a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetTerminalBalls.path b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetTerminalBalls.path
index 62a4e964..59264196 100644
--- a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetTerminalBalls.path
+++ b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetTerminalBalls.path
@@ -1,3 +1,3 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
 5.3,-6.2296,-2.0,0.0,true,false,
-1.29,-6.8896,-1.6,-1.96,true,false,
+1.2,-6.9696,-2.5,-3.0,true,false,
diff --git a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetWallBall.path b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetWallBall.path
index 22bd81ba..c7936eb5 100644
--- a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetWallBall.path
+++ b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallGetWallBall.path
@@ -1,3 +1,3 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
 7.7,-6.5496,0.0,-1.2,true,false,
-7.7,-7.6796,0.0,-1.2,true,false,
+7.7,-7.7296,0.0,-1.2,true,false,
diff --git a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallShootTerminalBalls.path b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallShootTerminalBalls.path
index d884972e..7d2923e3 100644
--- a/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallShootTerminalBalls.path
+++ b/PathWeaver/FiveBallAuton/PathWeaver/Paths/FiveBallShootTerminalBalls.path
@@ -1,3 +1,3 @@
 X,Y,Tangent X,Tangent Y,Fixed Theta,Reversed,Name
-1.29,-6.8896,-1.74,-1.43,true,true,
+1.2,-6.9696,-1.5,-1.75,true,true,
 7.9,-6.529599999999999,-0.75,-3.0,true,true,
diff --git a/limelights/Hofstra.vpr b/limelights/Hofstra.vpr
new file mode 100644
index 00000000..d094e5aa
--- /dev/null
+++ b/limelights/Hofstra.vpr
@@ -0,0 +1,68 @@
+area_max:10
+area_min:0
+area_similarity:60
+aspect_max:4
+aspect_min:1.0
+black_level:0
+blue_balance:500
+calibration_type:0
+contour_grouping:2
+contour_sort_final:2
+convexity_max:100
+convexity_min:50
+corner_approx:5.000000
+crop_x_max:1
+crop_x_min:-1.000000
+crop_y_max:0.8
+crop_y_min:-0.45
+cross_a_a:1
+cross_a_x:0
+cross_a_y:0
+cross_b_a:1
+cross_b_x:0
+cross_b_y:0
+desc:Hofstra
+desired_contour_region:0
+dilation_steps:1
+direction_filter:0
+dual_close_sort_origin:0
+erosion_steps:0
+exposure:2
+force_convex:1
+hue_max:60
+hue_min:45
+image_flip:0
+image_source:0
+img_to_show:1
+intersection_filter:0
+invert_hue:0
+multigroup_max:3
+multigroup_min:3
+multigroup_rejector:0
+pipeline_led_enabled:1
+pipeline_led_power:100
+pipeline_res:1
+pipeline_type:0
+red_balance:2500
+roi_x:0.000000
+roi_y:0.000000
+sat_max:255
+sat_min:190
+send_corners:0
+send_raw_contours:0
+solve3d:0
+solve3d_algo:0
+solve3d_bindtarget:1
+solve3d_conf:0.990000
+solve3d_error:8
+solve3d_guess:0
+solve3d_iterations:50
+solve3d_precies:1
+solve3d_precise:0
+solve3d_zoffset:0.000000
+val_max:255
+val_min:190
+x_outlier_miqr:0.1
+y_max:1.000000
+y_min:-1.000000
+y_outlier_miqr:0.2
diff --git a/limelights/test.vpr b/limelights/test.vpr
new file mode 100644
index 00000000..eeeb163a
--- /dev/null
+++ b/limelights/test.vpr
@@ -0,0 +1,64 @@
+area_max:10
+area_min:0
+area_similarity:24
+aspect_max:5
+aspect_min:1
+black_level:0
+blue_balance:500
+calibration_type:0
+contour_grouping:2
+contour_sort_final:2
+convexity_max:100
+convexity_min:20
+corner_approx:5.000000
+cross_a_a:1
+cross_a_x:0
+cross_a_y:0
+cross_b_a:1
+cross_b_x:0
+cross_b_y:0
+desc:Pipeline_Name
+desired_contour_region:0
+dilation_steps:1
+direction_filter:0
+dual_close_sort_origin:0
+erosion_steps:0
+exposure:2
+force_convex:1
+hue_max:76
+hue_min:35
+image_flip:0
+image_source:0
+img_to_show:1
+intersection_filter:0
+invert_hue:0
+multigroup_max:3
+multigroup_min:3
+multigroup_rejector:1
+pipeline_led_enabled:1
+pipeline_led_power:100
+pipeline_res:1
+pipeline_type:0
+red_balance:2500
+roi_x:0.000000
+roi_y:0.000000
+sat_max:255
+sat_min:200
+send_corners:0
+send_raw_contours:0
+solve3d:0
+solve3d_algo:0
+solve3d_bindtarget:1
+solve3d_conf:0.990000
+solve3d_error:8
+solve3d_guess:0
+solve3d_iterations:50
+solve3d_precies:1
+solve3d_precise:0
+solve3d_zoffset:0.000000
+val_max:255
+val_min:150
+x_outlier_miqr:0.1
+y_max:1.000000
+y_min:-1.000000
+y_outlier_miqr:0.5
diff --git a/shuffleboard.json b/shuffleboard.json
index 0ea37c3d..1d22f086 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -43,6 +43,17 @@
               "_source0": "network_table:///SmartDashboard/Pump/Compressor Enabled",
               "_title": "SmartDashboard/Pump/Compressor Enabled"
             }
+          },
+          "9,1": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Toggle Switch",
+              "_source0": "network_table:///SmartDashboard/Swap Rainbow",
+              "_title": "Rainbow Swap?"
+            }
           }
         }
       }
@@ -1300,9 +1311,9 @@
     }
   ],
   "windowGeometry": {
-    "x": -7.199999809265137,
-    "y": -7.199999809265137,
-    "width": 1550.4000244140625,
-    "height": 838.4000244140625
+    "x": -6.400000095367432,
+    "y": 1.600000023841858,
+    "width": 1547.199951171875,
+    "height": 828.7999877929688
   }
 }
\ No newline at end of file
diff --git a/src/main/deploy/FiveBallAuton/output/FiveBallGetTerminalBalls.wpilib.json b/src/main/deploy/FiveBallAuton/output/FiveBallGetTerminalBalls.wpilib.json
index f1850419..b466a804 100644
--- a/src/main/deploy/FiveBallAuton/output/FiveBallGetTerminalBalls.wpilib.json
+++ b/src/main/deploy/FiveBallAuton/output/FiveBallGetTerminalBalls.wpilib.json
@@ -1,6 +1,6 @@
 [
 {
-"acceleration": 2.9999999999999996,
+"acceleration": 3.0000000000000004,
 "curvature": -0.0,
 "pose": {
 "rotation": {
@@ -15,768 +15,783 @@
 "velocity": 0.0
 },
 {
-"acceleration": 3.000000000000002,
-"curvature": -0.04319056035778972,
+"acceleration": 2.9999999999999996,
+"curvature": -0.17327426554718073,
 "pose": {
 "rotation": {
-"radians": 3.140050784693115
+"radians": 3.135604411552811
 },
 "translation": {
-"x": 5.236868796944618,
-"y": 2.0000342559814452
+"x": 5.236946538090706,
+"y": 2.0001310753822326
 }
 },
-"time": 0.20515232121409616,
-"velocity": 0.6154569636422884
+"time": 0.20502617418066177,
+"velocity": 0.6150785225419854
 },
 {
-"acceleration": 3.0000000000000013,
-"curvature": -0.053116584925938154,
+"acceleration": 2.9999999999999987,
+"curvature": -0.24120894744579532,
 "pose": {
 "rotation": {
-"radians": 3.1366684371872893
+"radians": 3.121293554050636
 },
 "translation": {
-"x": 5.170192279815674,
-"y": 2.000246276855469
+"x": 5.170777988433838,
+"y": 2.000976333618164
 }
 },
-"time": 0.29417462679386425,
-"velocity": 0.8825238803815929
+"time": 0.2935160942606334,
+"velocity": 0.8805482827819003
 },
 {
-"acceleration": 3.000000000000002,
-"curvature": -0.04112765963825831,
+"acceleration": 2.999999999999999,
+"curvature": -0.2265438354559569,
 "pose": {
 "rotation": {
-"radians": 3.1331484725721457
+"radians": 3.104137441893413
 },
 "translation": {
-"x": 5.097069031596184,
-"y": 2.0007405471801754
+"x": 5.0989270299673075,
+"y": 2.0030585551261906
 }
 },
-"time": 0.3678160638718825,
-"velocity": 1.1034481916156478
+"time": 0.36615904525224946,
+"velocity": 1.0984771357567484
 },
 {
-"acceleration": 3.0000000000000004,
-"curvature": -0.022270315088369182,
+"acceleration": 3.0000000000000018,
+"curvature": -0.17487645892559828,
 "pose": {
 "rotation": {
-"radians": 3.130560850449367
+"radians": 3.0880777271872284
 },
 "translation": {
-"x": 5.015256958007813,
-"y": 2.0015478515625
+"x": 5.019387817382812,
+"y": 2.0067065429687503
 }
 },
-"time": 0.4356979396400502,
-"velocity": 1.307093818920151
+"time": 0.43261337162058017,
+"velocity": 1.2978401148617404
 },
 {
-"acceleration": 2.999999999999998,
-"curvature": -0.0047721559539036125,
+"acceleration": 3.0,
+"curvature": -0.11767316482058177,
 "pose": {
 "rotation": {
-"radians": 3.1293563460971816
+"radians": 3.0751530831124825
 },
 "translation": {
-"x": 4.923125866055488,
-"y": 2.0026321411132813
+"x": 4.930676284432411,
+"y": 2.012071430683136
 }
 },
-"time": 0.5012560983074843,
-"velocity": 1.5037682949224533
+"time": 0.49639037151075766,
+"velocity": 1.489171114532273
 },
 {
 "acceleration": 2.999999999999999,
-"curvature": 0.00898569815841764,
+"curvature": -0.06825143192507822,
 "pose": {
 "rotation": {
-"radians": 3.1296212052032772
+"radians": 3.0660664286073223
 },
 "translation": {
-"x": 4.819610042572021,
-"y": 2.0038973999023435
+"x": 4.831790447235107,
+"y": 2.0191429901123046
 }
 },
-"time": 0.5659270093495695,
-"velocity": 1.697781028048709
+"time": 0.5590130994815661,
+"velocity": 1.6770392984446982
 },
 {
 "acceleration": 3.0,
-"curvature": 0.019263998810139113,
+"curvature": -0.02937382592298787,
 "pose": {
 "rotation": {
-"radians": 3.131291398497853
+"radians": 3.0608281135434905
 },
 "translation": {
-"x": 4.704160832762718,
-"y": 2.005194511413574
+"x": 4.722170707583428,
+"y": 2.027765939235687
 }
 },
-"time": 0.6302732561849533,
-"velocity": 1.8908197685548604
+"time": 0.621128991187707,
+"velocity": 1.863386973563121
 },
 {
-"acceleration": 3.0000000000000018,
-"curvature": 0.02340150241481691,
+"acceleration": 3.000000000000012,
+"curvature": 0.0003657279112970315,
 "pose": {
 "rotation": {
-"radians": 3.1326231705674683
+"radians": 3.0591815711319055
 },
 "translation": {
-"x": 4.64191946638748,
-"y": 2.005795583128929
+"x": 4.6016601562499995,
+"y": 2.0376562499999995
 }
 },
-"time": 0.6623749365268162,
-"velocity": 1.9871248095804492
+"time": 0.6829434115512043,
+"velocity": 2.0488302346536127
 },
 {
-"acceleration": 2.9999999999999982,
-"curvature": 0.02704219822985778,
+"acceleration": 3.000000000000005,
+"curvature": 0.012560260435248605,
 "pose": {
 "rotation": {
-"radians": 3.1342714702211776
+"radians": 3.0596093896517775
 },
 "translation": {
-"x": 4.57669921875,
-"y": 2.0063281249999996
+"x": 4.537372133228928,
+"y": 2.042956716492772
 }
 },
-"time": 0.6944221850274832,
-"velocity": 2.0832665550824503
+"time": 0.7137337115001476,
+"velocity": 2.141201134500443
 },
 {
-"acceleration": 2.999999999999997,
-"curvature": 0.03030158950966123,
+"acceleration": 3.0000000000000018,
+"curvature": 0.02336941058087589,
 "pose": {
 "rotation": {
-"radians": 3.1362279556524384
+"radians": 3.060824664292072
 },
 "translation": {
-"x": 4.508555813301355,
-"y": 2.006761620640755
+"x": 4.470464876294136,
+"y": 2.048417456150055
 }
 },
-"time": 0.7263966046487365,
-"velocity": 2.17918981394621
+"time": 0.7444252578697539,
+"velocity": 2.233275773609262
 },
 {
-"acceleration": 2.9999999999999956,
-"curvature": 0.033281792158017603,
+"acceleration": 3.0,
+"curvature": 0.03306103102807454,
 "pose": {
 "rotation": {
-"radians": 3.1384868552414558
+"radians": 3.0627982689225415
 },
 "translation": {
-"x": 4.437568398118019,
-"y": 2.007063522338867
+"x": 4.401014350634068,
+"y": 2.0539740177243946
 }
 },
-"time": 0.758272641392119,
-"velocity": 2.2748179241763578
+"time": 0.7749950112320265,
+"velocity": 2.32498503369608
 },
 {
-"acceleration": 3.000000000000003,
-"curvature": 0.03607198123985722,
+"acceleration": 3.000000000000009,
+"curvature": 0.04187346844992513,
 "pose": {
 "rotation": {
-"radians": 3.141045139565368
+"radians": 3.065506655083698
 },
 "translation": {
-"x": 4.363838063981384,
-"y": 2.0071994656324383
+"x": 4.329114246368408,
+"y": 2.0595569610595703
 }
 },
-"time": 0.7900196442653311,
-"velocity": 2.370058932795994
+"time": 0.805416014070402,
+"velocity": 2.4162480422112065
 },
 {
-"acceleration": 3.0000000000000027,
-"curvature": 0.03874999183794059,
+"acceleration": 2.999999999999991,
+"curvature": 0.05001656468571273,
 "pose": {
 "rotation": {
-"radians": -3.139282717120867
+"radians": 3.068932151913591
 },
 "translation": {
-"x": 4.287486362457275,
-"y": 2.0071334838867188
+"x": 4.254874738026411,
+"y": 2.0650923667103047
 }
 },
-"time": 0.8216034272574952,
-"velocity": 2.4648102817724866
+"time": 0.8356586304020694,
+"velocity": 2.506975891206209
 },
 {
-"acceleration": 2.999999999999989,
-"curvature": 0.04138437971210569,
+"acceleration": 3.000000000000011,
+"curvature": 0.05767462230889834,
 "pose": {
 "rotation": {
-"radians": -3.1361234887026224
+"radians": 3.073063046743287
 },
 "translation": {
-"x": 4.208653823975473,
-"y": 2.0068282228708263
+"x": 4.17842124402523,
+"y": 2.070502345561981
 }
 },
-"time": 0.8529874625996534,
-"velocity": 2.558962387798961
+"time": 0.8656915095181289,
+"velocity": 2.597074528554387
 },
 {
-"acceleration": 3.0,
-"curvature": 0.044036577170958234,
+"acceleration": 2.9999999999999947,
+"curvature": 0.06501018953216565,
 "pose": {
 "rotation": {
-"radians": -3.1326570348689584
+"radians": 3.077893545309822
 },
 "translation": {
-"x": 4.127498475909233,
-"y": 2.0062451553344722
+"x": 4.099893186148256,
+"y": 2.075705548450351
 }
 },
-"time": 0.8841337981764168,
-"velocity": 2.652401394529251
+"time": 0.895482340599228,
+"velocity": 2.686447021797685
 },
 {
-"acceleration": 3.0000000000000027,
-"curvature": 0.0467629740634558,
+"acceleration": 2.9999999999999973,
+"curvature": 0.07216802672298654,
 "pose": {
 "rotation": {
-"radians": -3.1288750584812597
+"radians": 3.0834236763948475
 },
 "translation": {
-"x": 4.044194360654801,
-"y": 2.005344795584678
+"x": 4.019442749023437,
+"y": 2.08061767578125
 }
 },
-"time": 0.915003766958062,
-"velocity": 2.7450113008741863
+"time": 0.924998447915503,
+"velocity": 2.7749953437465096
 },
 {
-"acceleration": 2.9999999999999925,
-"curvature": 0.0496168640685434,
+"acceleration": 2.9999999999999947,
+"curvature": 0.07927892463142762,
 "pose": {
 "rotation": {
-"radians": -3.1247663759008995
+"radians": 3.0896591820478
 },
 "translation": {
-"x": 3.958930053710937,
-"y": 2.0040869140624995
+"x": 3.937233639601618,
+"y": 2.0851519871503115
 }
 },
-"time": 0.9455585387678223,
-"velocity": 2.8366756163034674
+"time": 0.9542072640949807,
+"velocity": 2.8626217922849424
 },
 {
-"acceleration": 3.000000000000005,
-"curvature": 0.0526502564982794,
+"acceleration": 3.000000000000013,
+"curvature": 0.08646322424663715,
 "pose": {
 "rotation": {
-"radians": -3.120316847801895
+"radians": 3.0966114183010216
 },
 "translation": {
-"x": 3.8719071817584334,
-"y": 2.0024307519197464
+"x": 3.8534398466348647,
+"y": 2.0892198109626765
 }
 },
-"time": 0.9757595521356515,
-"velocity": 2.927278656406955
+"time": 0.9830767100305983,
+"velocity": 2.949230130091795
 },
 {
 "acceleration": 3.0,
-"curvature": 0.055915585080355205,
+"curvature": 0.09383398794794387,
 "pose": {
 "rotation": {
-"radians": -3.11550927124633
+"radians": 3.1042972800854014
 },
 "translation": {
-"x": 3.783338940739631,
-"y": 2.0003352355957027
+"x": 3.7682444001547992,
+"y": 2.0927310540527104
 }
 },
-"time": 1.0055688548567434,
-"velocity": 3.0167065645702307
+"time": 1.011575503404274,
+"velocity": 3.0347265102128222
 },
 {
-"acceleration": 2.6452053853216158,
-"curvature": 0.05946736070251709,
+"acceleration": 0.5096512415275684,
+"curvature": 0.1014998221376482,
 "pose": {
 "rotation": {
-"radians": -3.1103232282702264
+"radians": 3.112739156177471
 },
 "translation": {
-"x": 3.693448613937944,
-"y": 1.9977591913938522
+"x": 3.6818381309509274,
+"y": 2.0955947113037112
 }
 },
-"time": 1.0349493751486105,
-"velocity": 3.104848125445832
+"time": 1.039673412874167,
+"velocity": 3.119020238622501
 },
 {
-"acceleration": -0.15745638086015995,
-"curvature": 0.06336382259852937,
+"acceleration": -0.3114512194952281,
+"curvature": 0.10956737194591815,
 "pose": {
 "rotation": {
-"radians": -3.1047348870133034
+"radians": 3.121964914184875
 },
 "translation": {
-"x": 3.6024680900573722,
-"y": 1.994661560058593
+"x": 3.594418430048972,
+"y": 2.0977193752676246
 }
 },
-"time": 1.063911763103943,
-"velocity": 3.181459590037051
+"time": 1.0676456995265606,
+"velocity": 3.1332763492432587
 },
 {
-"acceleration": -0.17300714515723534,
-"curvature": 0.06766864635733778,
+"acceleration": -0.3287657034856548,
+"curvature": 0.11814351022309137,
 "pose": {
 "rotation": {
-"radians": -3.0987167516795275
+"radians": 3.1320079108695422
 },
 "translation": {
-"x": 3.5106363813020285,
-"y": 1.9910016113519662
+"x": 3.506188008189201,
+"y": 2.0990137457847595
 }
 },
-"time": 1.0928200056402244,
-"velocity": 3.1769078027902604
+"time": 1.0958474156332758,
+"velocity": 3.124492890369964
 },
 {
-"acceleration": -0.19146404489979324,
-"curvature": 0.07245276987034426,
+"acceleration": -0.35002072163025855,
+"curvature": 0.1273372321527724,
 "pose": {
 "rotation": {
-"radians": -3.0922373575733455
+"radians": -3.1402782883771736
 },
 "translation": {
-"x": 3.4181981414556497,
-"y": 1.986739158630371
+"x": 3.4173536553047597,
+"y": 2.099387139603495
 }
 },
-"time": 1.1219709839241834,
-"velocity": 3.171864475258812
+"time": 1.124321930386232,
+"velocity": 3.1151314464957958
 },
 {
-"acceleration": -0.21326027494213795,
-"curvature": 0.07779640109258544,
+"acceleration": -0.37528574249034297,
+"curvature": 0.13726124640003187,
 "pose": {
 "rotation": {
-"radians": -3.0852609073030375
+"radians": -3.128478651312318
 },
 "translation": {
-"x": 3.3254021839611227,
-"y": 1.981834773421287
+"x": 3.328125,
+"y": 2.09875
 }
 },
-"time": 1.1512937314780152,
-"velocity": 3.16625022340458
+"time": 1.1530125302651757,
+"velocity": 3.105089142022163
 },
 {
-"acceleration": -0.23893675503214282,
-"curvature": 0.08379127466206349,
+"acceleration": -0.4046712195409514,
+"curvature": 0.14803322454448273,
 "pose": {
 "rotation": {
-"radians": -3.077746844143513
+"radians": -3.115728508186169
 },
 "translation": {
-"x": 3.2324999999999995,
-"y": 1.9762499999999994
+"x": 3.2387132690288123,
+"y": 2.097014406397939
 }
 },
-"time": 1.1807172461748776,
-"velocity": 3.159975356570563
+"time": 1.1818634778394583,
+"velocity": 3.0942617927401983
 },
 {
-"acceleration": -0.26915736476827995,
-"curvature": 0.0905432277255727,
+"acceleration": -0.43830250190516257,
+"curvature": 0.1597766319753005,
 "pose": {
 "rotation": {
-"radians": -3.069649358683333
+"radians": -3.1019723499293854
 },
 "translation": {
-"x": 3.139744276572018,
-"y": 1.9699475699663154
+"x": 3.149330046772957,
+"y": 2.0940945839881895
 }
 },
-"time": 1.2101710305762439,
-"velocity": 3.1529377649022843
+"time": 1.2108204845726656,
+"velocity": 3.082543725511216
 },
 {
-"acceleration": -0.30472705790758525,
-"curvature": 0.09817516780345656,
+"acceleration": -0.4762882645133464,
+"curvature": 0.1726210138443533,
 "pose": {
 "rotation": {
-"radians": -3.0609168254645485
+"radians": -3.087149235523504
 },
 "translation": {
-"x": 3.0473874145746223,
-"y": 1.9628916168212882
+"x": 3.060186034720391,
+"y": 2.0899074133485556
 }
 },
-"time": 1.2395856432368264,
-"velocity": 3.145020605272882
+"time": 1.2398311801242365,
+"velocity": 3.0698282650689532
 },
 {
-"acceleration": -0.3466120625782205,
-"curvature": 0.10683050533115111,
+"acceleration": -0.518680705583067,
+"curvature": 0.18670154703001984,
 "pose": {
 "rotation": {
-"radians": -3.0514911676565224
+"radians": -3.071193007837991
 },
 "translation": {
-"x": 2.9556800468824798,
-"y": 1.9550478905439368
+"x": 2.9714898109436034,
+"y": 2.084372940063476
 }
 },
-"time": 1.2688932616516222,
-"velocity": 3.1360897809390633
+"time": 1.2688455733202826,
+"velocity": 3.0560090500877006
 },
 {
-"acceleration": -0.39596065009321535,
-"curvature": 0.11667711744389286,
+"acceleration": -0.5654246552437026,
+"curvature": 0.2021575914736444,
 "pose": {
 "rotation": {
-"radians": -3.0413071503055416
+"radians": -3.05403267946703
 },
 "translation": {
-"x": 2.864869556427001,
-"y": 1.9463839721679683
+"x": 2.883446589577943,
+"y": 2.077414884343743
 }
 },
-"time": 1.2980282572924446,
-"velocity": 3.1259912400067904
+"time": 1.2978164983483818,
+"velocity": 3.040982390252732
 },
 {
-"acceleration": -1.9397306143287538,
-"curvature": 0.12791189236314388,
+"acceleration": -0.6162928631238812,
+"curvature": 0.21912988097035063,
 "pose": {
 "rotation": {
-"radians": -3.03029160697016
+"radians": -3.035593057416685
 },
 "translation": {
-"x": 2.7751985942758615,
-"y": 1.9368694883584974
+"x": 2.7962569802999497,
+"y": 2.06896115064621
 }
 },
-"time": 1.3269277827559567,
-"velocity": 3.1145481651168727
+"time": 1.3267000377149838,
+"velocity": 3.024650924964153
 },
 {
-"acceleration": -3.0,
-"curvature": 0.14076586696536944,
+"acceleration": -0.6708053087274829,
+"curvature": 0.23775588649305585,
 "pose": {
 "rotation": {
-"radians": -3.0183626114438913
+"radians": -3.015795692476112
 },
 "translation": {
-"x": 2.686903597712516,
-"y": 1.9264763259887694
+"x": 2.710115747805685,
+"y": 2.0589443372935055
 }
 },
-"time": 1.355731064349443,
-"velocity": 3.0586775580168557
+"time": 1.3554559110516147,
+"velocity": 3.006928885453893
 },
 {
-"acceleration": -3.0000000000000053,
-"curvature": 0.15550990051178923,
+"acceleration": -0.7281317015889168,
+"curvature": 0.2581627715241391,
 "pose": {
 "rotation": {
-"radians": -3.0054286169954456
+"radians": -2.994560258119386
 },
 "translation": {
-"x": 2.6002133083157224,
-"y": 1.9151788467168807
+"x": 2.6252105712890623,
+"y": 2.04730224609375
 }
 },
-"time": 1.3847254058570113,
-"velocity": 2.9716945334941505
+"time": 1.3840478159682823,
+"velocity": 2.987749283849161
 },
 {
-"acceleration": -3.0000000000000107,
-"curvature": 0.17246070468332195,
+"acceleration": -0.7869787480272464,
+"curvature": 0.28045725450831815,
 "pose": {
 "rotation": {
-"radians": -2.9913876018190946
+"radians": -2.9718064836451314
 },
 "translation": {
-"x": 2.515347290039061,
-"y": 1.9029541015624991
+"x": 2.54172080392018,
+"y": 2.0339783919602636
 }
 },
-"time": 1.4140112077795879,
-"velocity": 2.8838371277264203
+"time": 1.4124437039002102,
+"velocity": 2.967073337651158
 },
 {
-"acceleration": -3.0,
-"curvature": 0.19198684331080818,
+"acceleration": -0.8454675617660978,
+"curvature": 0.30471162772750326,
 "pose": {
 "rotation": {
-"radians": -2.976126283513142
+"radians": -2.9474567846686313
 },
 "translation": {
-"x": 2.4325144472904494,
-"y": 1.8897820454835887
+"x": 2.4598162323236465,
+"y": 2.0189225125312813
 }
 },
-"time": 1.4435490545676488,
-"velocity": 2.7952235873622375
+"time": 1.4406159704616768,
+"velocity": 2.9449023625835253
 },
 {
-"acceleration": -3.0000000000000058,
-"curvature": 0.21451397974673062,
+"acceleration": -2.8581739391107863,
+"curvature": 0.3309452027872177,
 "pose": {
 "rotation": {
-"radians": -2.959519500507119
+"radians": -2.9214397471345497
 },
 "translation": {
-"x": 2.3519115430116635,
-"y": 1.8756457519531242
+"x": 2.3796558360569184,
+"y": 2.0020910777896646
 }
 },
-"time": 1.4733001015366867,
-"velocity": 2.705970446455124
+"time": 1.4685415365629002,
+"velocity": 2.921292202300986
 },
 {
-"acceleration": -2.9999999999999942,
-"curvature": 0.24052812304413496,
+"acceleration": -2.999999999999989,
+"curvature": 0.35910063188707525,
 "pose": {
 "rotation": {
-"radians": -2.941429908444905
+"radians": -2.893694622717461
 },
 "translation": {
-"x": 2.273721716757863,
-"y": 1.8605316275358197
+"x": 2.3013865470886232,
+"y": 1.9834477996826179
 }
 },
-"time": 1.5032267473547622,
-"velocity": 2.616190509000897
+"time": 1.4964652493085597,
+"velocity": 2.841481374248126
 },
 {
 "acceleration": -2.9999999999999942,
-"curvature": 0.2705748236598588,
+"curvature": 0.389014985555292,
 "pose": {
 "rotation": {
-"radians": -2.9217082094582447
+"radians": -2.8641769753789736
 },
 "translation": {
-"x": 2.198113002777098,
-"y": 1.8444296264648434
+"x": 2.2251420092768965,
+"y": 1.9629641417413959
 }
 },
-"time": 1.5332934777736162,
-"velocity": 2.5259903177443355
+"time": 1.5246693232863104,
+"velocity": 2.756869152314874
 },
 {
-"acceleration": -2.999999999999994,
-"curvature": 0.30525109802056855,
+"acceleration": -3.0,
+"curvature": 0.42038625798466595,
 "pose": {
 "rotation": {
-"radians": -2.9001942273383365
+"radians": -2.83286557020452
 },
 "translation": {
-"x": 2.125236848089842,
-"y": 1.827333465218544
+"x": 2.15104133784771,
+"y": 1.9406198287010197
 }
 },
-"time": 1.5634679227205817,
-"velocity": 2.4354669829034394
+"time": 1.5531857357290697,
+"velocity": 2.6713199149865967
 },
 {
-"acceleration": -3.000000000000006,
-"curvature": 0.34518522736723734,
+"acceleration": -2.9999999999999942,
+"curvature": 0.45273720831866104,
 "pose": {
 "rotation": {
-"radians": -2.8767192664573713
+"radians": -2.7997705045160455
 },
 "translation": {
-"x": 2.0552266305685043,
-"y": 1.8092408370971675
+"x": 2.0791878788732,
+"y": 1.9164033561199911
 }
 },
-"time": 1.593722178193401,
-"velocity": 2.3447042164849816
+"time": 1.5820378324131208,
+"velocity": 2.5847636249344434
 },
 {
-"acceleration": -2.9999999999999933,
-"curvature": 0.3909974469474708,
+"acceleration": -3.0,
+"curvature": 0.4853801406658371,
 "pose": {
 "rotation": {
-"radians": -2.8511103483893487
+"radians": -2.7649424372775733
 },
 "translation": {
-"x": 1.9881961770169436,
-"y": 1.790153626799583
+"x": 2.0096679687500005,
+"y": 1.8903125000000003
 }
 },
-"time": 1.6240344557383461,
-"velocity": 2.253767383850146
+"time": 1.6112612577969374,
+"velocity": 2.4970933487829936
 },
 {
-"acceleration": -2.9999999999999982,
-"curvature": 0.44323205429929224,
+"acceleration": -2.999999999999994,
+"curvature": 0.5173882225287005,
 "pose": {
 "rotation": {
-"radians": -2.823197101206156
+"radians": -2.7284825720865595
 },
 "translation": {
-"x": 1.9242382812499992,
-"y": 1.7700781249999995
+"x": 1.9425496936775746,
+"y": 1.862354826405645
 }
 },
-"time": 1.654391136333484,
-"velocity": 2.162697342064733
+"time": 1.6409063364182368,
+"velocity": 2.4081581129190957
 },
 {
-"acceleration": -2.9999999999999982,
-"curvature": 0.5680630644136069,
+"acceleration": -3.000000000000006,
+"curvature": 0.5475808229934561,
 "pose": {
 "rotation": {
-"radians": -2.759856877904301
+"radians": -2.690552801201823
 },
 "translation": {
-"x": 1.8057972818613033,
-"y": 1.727010726928711
+"x": 1.877881649136543,
+"y": 1.8325482010841379
 }
 },
-"time": 1.7152320498667124,
-"velocity": 1.9801746014650479
+"time": 1.671040909256718,
+"velocity": 2.3177543944036523
 },
 {
-"acceleration": -2.999999999999998,
-"curvature": 0.7169971953049145,
+"acceleration": -3.0,
+"curvature": 0.5745313837844774,
 "pose": {
 "rotation": {
-"radians": -2.6859129894596334
+"radians": -2.6513851540127513
 },
 "translation": {
-"x": 1.7001690101623508,
-"y": 1.6801852416992196
+"x": 1.815691699367017,
+"y": 1.8009212990850223
 }
 },
-"time": 1.776417295341532,
-"velocity": 1.7966188650405888
+"time": 1.7017537864424206,
+"velocity": 2.225615762846544
 },
 {
-"acceleration": -3.0,
-"curvature": 0.8732806682987305,
+"acceleration": -2.9999999999999933,
+"curvature": 0.5966055464914407,
 "pose": {
 "rotation": {
-"radians": -2.601863544096133
+"radians": -2.6112894703289498
 },
 "translation": {
-"x": 1.6071874731779099,
-"y": 1.6298324966430675
+"x": 1.7559857368469247,
+"y": 1.7675141143798854
 }
 },
-"time": 1.8384891441954379,
-"velocity": 1.6104033184788713
+"time": 1.7331590805662247,
+"velocity": 2.131399880475132
 },
 {
-"acceleration": -3.000000000000005,
-"curvature": 0.9427014555912406,
+"acceleration": -2.9999999999999964,
+"curvature": 0.6120337455299701,
 "pose": {
 "rotation": {
-"radians": -2.5568345551894534
+"radians": -2.5706581171962313
 },
 "translation": {
-"x": 1.5652577343396823,
-"y": 1.6034302562475213
+"x": 1.6987464417703455,
+"y": 1.7323784694820663
 }
 },
-"time": 1.8701939853930256,
-"velocity": 1.5152887948861078
+"time": 1.765401861999556,
+"velocity": 2.0346715361751384
 },
 {
-"acceleration": -2.999999999999998,
-"curvature": 0.9967861343736467,
+"acceleration": -3.0000000000000036,
+"curvature": 0.6190159477557204,
 "pose": {
 "rotation": {
-"radians": -2.510636445914125
+"radians": -2.5299666740648212
 },
 "translation": {
-"x": 1.5262078857421821,
-"y": 1.5762744140625014
+"x": 1.6439320415258427,
+"y": 1.6955785250663773
 }
 },
-"time": 1.9026244813755055,
-"velocity": 1.417997306938668
+"time": 1.7986658863619283,
+"velocity": 1.9348794630880215
 },
 {
 "acceleration": -3.0,
-"curvature": 1.0266487525275627,
+"curvature": 0.6158475944421496,
 "pose": {
 "rotation": {
-"radians": -2.4642083336537906
+"radians": -2.4897698853903427
 },
 "translation": {
-"x": 1.489874737877396,
-"y": 1.5484205704927465
+"x": 1.5914750701747833,
+"y": 1.6571912895888108
 }
 },
-"time": 1.936095517979685,
-"velocity": 1.3175841971261297
+"time": 1.8331846956896816,
+"velocity": 1.8313230351047614
 },
 {
-"acceleration": -3.0000000000000004,
-"curvature": 1.0229167434261608,
+"acceleration": -3.0,
+"curvature": 0.6010474639469057,
 "pose": {
 "rotation": {
-"radians": -2.4187504110102633
+"radians": -2.4506928167187514
 },
 "translation": {
-"x": 1.456059249043462,
-"y": 1.5199308776855487
+"x": 1.5412811279296887,
+"y": 1.617307128906253
 }
 },
-"time": 1.9710453224439795,
-"velocity": 1.2127347837332467
+"time": 1.8692584193586876,
+"velocity": 1.7231018640977431
 },
 {
-"acceleration": -3.0000000000000004,
-"curvature": 0.8819855475375235,
+"acceleration": -3.0,
+"curvature": 0.5323341764865099,
+"pose": {
+"rotation": {
+"radians": -2.378669727237611
+},
+"translation": {
+"x": 1.447162619233132,
+"y": 1.5334793400764495
+}
+},
+"time": 1.947769943033689,
+"velocity": 1.487567293072739
+},
+{
+"acceleration": -2.9999999999999996,
+"curvature": 0.4083193481751454,
 "pose": {
 "rotation": {
-"radians": -2.336686313758596
+"radians": -2.3197664038057617
 },
 "translation": {
-"x": 1.3949969291686974,
-"y": 1.4613265991210964
+"x": 1.360235500335695,
+"y": 1.445104598999027
 }
 },
-"time": 2.048195941482598,
-"velocity": 0.981282926617392
+"time": 2.039605866114473,
+"velocity": 1.2120595238303868
 },
 {
 "acceleration": -3.0000000000000004,
-"curvature": 0.5348716092698073,
+"curvature": 0.22967733020429182,
 "pose": {
 "rotation": {
-"radians": -2.2777225836707142
+"radians": -2.2800605288172564
 },
 "translation": {
-"x": 1.340655235648156,
-"y": 1.4010979843139681
+"x": 1.2786484211683282,
+"y": 1.35343868017197
 }
 },
-"time": 2.1452674773222746,
-"velocity": 0.6900683190983617
+"time": 2.158280821956625,
+"velocity": 0.8560346563039307
 },
 {
 "acceleration": -3.0000000000000004,
-"curvature": -4.4219918198383454e-15,
+"curvature": -1.9089977693525934e-15,
 "pose": {
 "rotation": {
-"radians": -2.2554134911076833
+"radians": -2.2655346029916017
 },
 "translation": {
-"x": 1.2899999999999983,
-"y": 1.3400000000000043
+"x": 1.200000000000002,
+"y": 1.2600000000000051
 }
 },
-"time": 2.3752902503550617,
+"time": 2.4436257073912686,
 "velocity": 0.0
 }
 ]
\ No newline at end of file
diff --git a/src/main/deploy/FiveBallAuton/output/FiveBallGetWallBall.wpilib.json b/src/main/deploy/FiveBallAuton/output/FiveBallGetWallBall.wpilib.json
index 4a68327e..71033cc3 100644
--- a/src/main/deploy/FiveBallAuton/output/FiveBallGetWallBall.wpilib.json
+++ b/src/main/deploy/FiveBallAuton/output/FiveBallGetWallBall.wpilib.json
@@ -15,7 +15,7 @@
 "velocity": 0.0
 },
 {
-"acceleration": 2.9999999999999987,
+"acceleration": 2.9999999999999996,
 "curvature": 0.0,
 "pose": {
 "rotation": {
@@ -23,14 +23,14 @@
 },
 "translation": {
 "x": 7.7,
-"y": 1.605155277252197
+"y": 1.6050443649291992
 }
 },
-"time": 0.22337520415629933,
-"velocity": 0.6701256124688981
+"time": 0.22354065263511613,
+"velocity": 0.6706219579053485
 },
 {
-"acceleration": 2.999999999999999,
+"acceleration": 3.0000000000000013,
 "curvature": 0.0,
 "pose": {
 "rotation": {
@@ -38,11 +38,11 @@
 },
 "translation": {
 "x": 7.7,
-"y": 1.531123657226562
+"y": 1.530321044921875
 }
 },
-"time": 0.3150411007825673,
-"velocity": 0.945123302347702
+"time": 0.31588917368609387,
+"velocity": 0.9476675210582817
 },
 {
 "acceleration": 3.0,
@@ -53,14 +53,14 @@
 },
 "translation": {
 "x": 7.7,
-"y": 1.458413829803467
+"y": 1.4559753799438475
 }
 },
-"time": 0.38434894752601456,
-"velocity": 1.1530468425780438
+"time": 0.3864579494642353,
+"velocity": 1.159373848392706
 },
 {
-"acceleration": 3.000000000000005,
+"acceleration": 2.9999999999999987,
 "curvature": 0.0,
 "pose": {
 "rotation": {
@@ -68,11 +68,11 @@
 },
 "translation": {
 "x": 7.7,
-"y": 1.38724609375
+"y": 1.3820703124999998
 }
 },
-"time": 0.44177966321836626,
-"velocity": 1.3253389896550989
+"time": 0.4456678041620985,
+"velocity": 1.3370034124862957
 },
 {
 "acceleration": 3.0,
@@ -83,11 +83,11 @@
 },
 "translation": {
 "x": 7.7,
-"y": 1.3176004219055173
+"y": 1.3086001205444333
 }
 },
-"time": 0.4915279429794962,
-"velocity": 1.474583828938489
+"time": 0.49759413143348485,
+"velocity": 1.4927823943004548
 },
 {
 "acceleration": 2.999999999999997,
@@ -98,14 +98,14 @@
 },
 "translation": {
 "x": 7.7,
-"y": 1.2492645263671882
+"y": 1.235504150390625
 }
 },
-"time": 0.5358703037320451,
-"velocity": 1.6076109111961356
+"time": 0.5443625321476945,
+"velocity": 1.6330875964430838
 },
 {
-"acceleration": 2.9999999999991998,
+"acceleration": 2.9999999999999267,
 "curvature": 0.0,
 "pose": {
 "rotation": {
@@ -113,14 +113,14 @@
 },
 "translation": {
 "x": 7.7,
-"y": 1.1818819236755376
+"y": 1.162680549621582
 }
 },
-"time": 0.5762627157379188,
-"velocity": 1.7287881472137565
+"time": 0.5872645345886398,
+"velocity": 1.7617936037659196
 },
 {
-"acceleration": -2.999999999999997,
+"acceleration": -3.0,
 "curvature": 0.0,
 "pose": {
 "rotation": {
@@ -128,11 +128,11 @@
 },
 "translation": {
 "x": 7.7,
-"y": 1.115000000000001
+"y": 1.0899999999999999
 }
 },
-"time": 0.6137317546507318,
-"velocity": 1.8411952639521656
+"time": 0.6271629240742258,
+"velocity": 1.8814887722226747
 },
 {
 "acceleration": -3.0,
@@ -143,11 +143,11 @@
 },
 "translation": {
 "x": 7.7,
-"y": 1.0481180763244646
+"y": 1.0173194503784186
 }
 },
-"time": 0.6512007935635452,
-"velocity": 1.7287881472137256
+"time": 0.6670613135598114,
+"velocity": 1.761793603765918
 },
 {
 "acceleration": -3.0,
@@ -158,14 +158,14 @@
 },
 "translation": {
 "x": 7.7,
-"y": 0.9807354736328158
+"y": 0.9444958496093756
 }
 },
-"time": 0.6915932055694185,
-"velocity": 1.6076109111961057
+"time": 0.7099633160007567,
+"velocity": 1.633087596443082
 },
 {
-"acceleration": -3.0000000000000018,
+"acceleration": -3.0,
 "curvature": 0.0,
 "pose": {
 "rotation": {
@@ -173,14 +173,14 @@
 },
 "translation": {
 "x": 7.7,
-"y": 0.9123995780944867
+"y": 0.8713998794555673
 }
 },
-"time": 0.7359355663219683,
-"velocity": 1.4745838289384563
+"time": 0.7567317167149664,
+"velocity": 1.4927823943004528
 },
 {
-"acceleration": -3.0,
+"acceleration": -2.9999999999999987,
 "curvature": 0.0,
 "pose": {
 "rotation": {
@@ -188,14 +188,14 @@
 },
 "translation": {
 "x": 7.7,
-"y": 0.8427539062500067
+"y": 0.7979296875000008
 }
 },
-"time": 0.7856838460830975,
-"velocity": 1.3253389896550687
+"time": 0.8086580439863529,
+"velocity": 1.3370034124862935
 },
 {
-"acceleration": -2.999999999999999,
+"acceleration": -2.9999999999999987,
 "curvature": 0.0,
 "pose": {
 "rotation": {
@@ -203,14 +203,14 @@
 },
 "translation": {
 "x": 7.7,
-"y": 0.7715861701965423
+"y": 0.7240246200561531
 }
 },
-"time": 0.8431145617754484,
-"velocity": 1.153046842578016
+"time": 0.8678678986842162,
+"velocity": 1.1593738483927036
 },
 {
-"acceleration": -2.9999999999999987,
+"acceleration": -2.9999999999999996,
 "curvature": 0.0,
 "pose": {
 "rotation": {
@@ -218,11 +218,11 @@
 },
 "translation": {
 "x": 7.7,
-"y": 0.698876342773449
+"y": 0.6496789550781266
 }
 },
-"time": 0.9124224085188959,
-"velocity": 0.9451233023476738
+"time": 0.9384366744623569,
+"velocity": 0.9476675210582817
 },
 {
 "acceleration": -3.0000000000000004,
@@ -233,11 +233,11 @@
 },
 "translation": {
 "x": 7.7,
-"y": 0.6248447227478184
+"y": 0.5749556350708023
 }
 },
-"time": 1.004088305145161,
-"velocity": 0.6701256124688783
+"time": 1.0307851955133347,
+"velocity": 0.6706219579053485
 },
 {
 "acceleration": -3.0000000000000004,
@@ -248,10 +248,10 @@
 },
 "translation": {
 "x": 7.7,
-"y": 0.5500000000000203
+"y": 0.5000000000000018
 }
 },
-"time": 1.2274635093014536,
+"time": 1.2543258481484507,
 "velocity": 0.0
 }
 ]
\ No newline at end of file
diff --git a/src/main/deploy/FiveBallAuton/output/FiveBallShootTerminalBalls.wpilib.json b/src/main/deploy/FiveBallAuton/output/FiveBallShootTerminalBalls.wpilib.json
index 767e292d..29182b25 100644
--- a/src/main/deploy/FiveBallAuton/output/FiveBallShootTerminalBalls.wpilib.json
+++ b/src/main/deploy/FiveBallAuton/output/FiveBallShootTerminalBalls.wpilib.json
@@ -1,14 +1,14 @@
 [
 {
-"acceleration": -2.9999999999999996,
+"acceleration": -3.0,
 "curvature": -0.0,
 "pose": {
 "rotation": {
-"radians": -2.453676329549598
+"radians": -2.279422598922567
 },
 "translation": {
-"x": 1.29,
-"y": 1.3399999999999999
+"x": 1.2,
+"y": 1.2599999999999998
 }
 },
 "time": 0.0,
@@ -16,1427 +16,1457 @@
 },
 {
 "acceleration": -3.0,
-"curvature": 1.3933940652051315,
+"curvature": 1.575504393626785,
 "pose": {
 "rotation": {
-"radians": -2.508356792801262
+"radians": -2.3414994642704334
 },
 "translation": {
-"x": 1.345906740128994,
-"y": 1.3841947671771049
+"x": 1.248475070297718,
+"y": 1.31416187018156
 }
 },
-"time": 0.2179683254419923,
-"velocity": -0.6539049763259768
+"time": 0.22013116054102225,
+"velocity": -0.6603934816230668
 },
 {
-"acceleration": -3.0000000000000013,
-"curvature": 1.7305789661996958,
+"acceleration": -3.000000000000001,
+"curvature": 2.0129448781799053,
 "pose": {
 "rotation": {
-"radians": -2.5675386083248584
+"radians": -2.410000390400045
 },
 "translation": {
-"x": 1.3766074562910944,
-"y": 1.4054103935230522
+"x": 1.2755829665344207,
+"y": 1.340301494495943
 }
 },
-"time": 0.2690518354636336,
-"velocity": -0.8071555063909006
+"time": 0.2712249979469631,
+"velocity": -0.8136749938408893
 },
 {
-"acceleration": -2.999999999999997,
-"curvature": 1.8144014170418383,
+"acceleration": -3.0000000000000013,
+"curvature": 2.172506056350764,
 "pose": {
 "rotation": {
-"radians": -2.6381540569432747
+"radians": -2.4933372504501627
 },
 "translation": {
-"x": 1.4104167461395265,
-"y": 1.4256314754486086
+"x": 1.3059393405914306,
+"y": 1.3653783321380617
 }
 },
-"time": 0.31408943710778764,
-"velocity": -0.9422683113233629
+"time": 0.3159315065293412,
+"velocity": -0.9477945195880237
 },
 {
-"acceleration": -3.0000000000000013,
-"curvature": 1.7055146487546224,
+"acceleration": -3.0000000000000027,
+"curvature": 2.15777756734404,
 "pose": {
 "rotation": {
-"radians": -2.7130204618298466
+"radians": -2.5379746526816787
 },
 "translation": {
-"x": 1.448161942800507,
-"y": 1.4445969809871162
+"x": 1.3226102557571722,
+"y": 1.3774292462735316
 }
 },
-"time": 0.35610888375680383,
-"velocity": -1.0683266512704113
+"time": 0.3369367281136595,
+"velocity": -1.0108101843409787
 },
 {
-"acceleration": -2.999999999999998,
-"curvature": 1.4870487273923665,
+"acceleration": -2.999999999999994,
+"curvature": 2.0920412983455234,
 "pose": {
 "rotation": {
-"radians": -2.7864493663222487
+"radians": -2.583296406437178
 },
 "translation": {
-"x": 1.4905703571438789,
-"y": 1.4620795837044716
+"x": 1.34040959184058,
+"y": 1.3891121903341261
 }
 },
-"time": 0.3967290754795264,
-"velocity": -1.190187226438579
+"time": 0.3573798711581816,
+"velocity": -1.0721396134745451
 },
 {
-"acceleration": -2.999999999999997,
-"curvature": 1.2322211932161915,
+"acceleration": -2.9999999999999916,
+"curvature": 1.9868193457703553,
 "pose": {
 "rotation": {
-"radians": -2.8547183491277126
+"radians": -2.628457949054429
 },
 "translation": {
-"x": 1.538272875295952,
-"y": 1.4778844188246874
+"x": 1.3594292815992957,
+"y": 1.4003976670317932
 }
 },
-"time": 0.4369161105436584,
-"velocity": -1.3107483316309751
+"time": 0.37744439155413434,
+"velocity": -1.1323331746624032
 },
 {
-"acceleration": -2.9999999999999964,
-"curvature": 0.9884473706858702,
+"acceleration": -3.0,
+"curvature": 1.8540473172053786,
 "pose": {
 "rotation": {
-"radians": -2.915927551711177
+"radians": -2.6727494811437436
 },
 "translation": {
-"x": 1.591807556152344,
-"y": 1.4918478393554686
+"x": 1.3797549828886986,
+"y": 1.4112583127617837
 }
 },
-"time": 0.4772625318682169,
-"velocity": -1.4317875956046504
+"time": 0.39727550846350096,
+"velocity": -1.191826525390503
 },
 {
-"acceleration": -2.999999999999995,
-"curvature": 0.7781791459787887,
+"acceleration": -3.000000000000003,
+"curvature": 1.5488409994374674,
 "pose": {
 "rotation": {
-"radians": -2.96952012537199
+"radians": -2.7566163622480913
 },
 "translation": {
-"x": 1.6516232288908212,
-"y": 1.503836172213778
+"x": 1.4246363779064268,
+"y": 1.4316060820315029
 }
 },
-"time": 0.5181212791033661,
-"velocity": -1.554363837310098
+"time": 0.43666940375044705,
+"velocity": -1.3100082112513411
 },
 {
-"acceleration": -3.000000000000006,
-"curvature": 0.607164392522563,
+"acceleration": -3.0000000000000058,
+"curvature": 1.244077454755678,
 "pose": {
 "rotation": {
-"radians": -3.0157592582004105
+"radians": -2.83206657943066
 },
 "translation": {
-"x": 1.7180830904841424,
-"y": 1.5137444743514061
+"x": 1.4756179809570313,
+"y": 1.4499777221679686
 }
 },
-"time": 0.5596837748501567,
-"velocity": -1.6790513245504697
+"time": 0.4762429601982231,
+"velocity": -1.4287288805946694
 },
 {
-"acceleration": -3.0000000000000027,
-"curvature": 0.47260502943883054,
+"acceleration": -3.0,
+"curvature": 0.9760938017778498,
 "pose": {
 "rotation": {
-"radians": -3.0553185629077393
+"radians": -2.8981022754954417
 },
 "translation": {
-"x": 1.791468303212896,
-"y": 1.5214952888805415
+"x": 1.5331710743252187,
+"y": 1.4662270010914655
 }
 },
-"time": 0.6020311635713418,
-"velocity": -1.8060934907140251
+"time": 0.5164068330179643,
+"velocity": -1.5492204990538931
 },
 {
-"acceleration": -2.999999999999995,
-"curvature": 0.36853667538388507,
+"acceleration": -2.999999999999998,
+"curvature": 0.7568638720225038,
 "pose": {
 "rotation": {
-"radians": -3.0890176928671655
+"radians": -2.954916263666016
 },
 "translation": {
-"x": 1.8719815921783447,
-"y": 1.5270374011993404
+"x": 1.597677756845951,
+"y": 1.4802379330992697
 }
 },
-"time": 0.645169792319614,
-"velocity": -1.9355093769588418
+"time": 0.557389612178092,
+"velocity": -1.6721688365342764
 },
 {
-"acceleration": -3.0000000000000036,
-"curvature": 0.28861740241310924,
+"acceleration": -2.9999999999999987,
+"curvature": 0.5847156780729873,
 "pose": {
 "rotation": {
-"radians": -3.117675184056009
+"radians": -3.0033237278259994
 },
 "translation": {
-"x": 1.959750842815265,
-"y": 1.5303445951174943
+"x": 1.669434682233259,
+"y": 1.4919234813470394
 }
 },
-"time": 0.689056179793124,
-"velocity": -2.0671685393793715
+"time": 0.5992923378391621,
+"velocity": -1.7978770135174866
 },
 {
 "acceleration": -3.0,
-"curvature": 0.22727611117814503,
+"curvature": 0.4524335499942336,
 "pose": {
 "rotation": {
-"radians": 3.1411469692217313
+"radians": -3.044374781056761
 },
 "translation": {
-"x": 2.054832698404789,
-"y": 1.5314144089818003
+"x": 1.7486567974090574,
+"y": 1.5012242603301997
 }
 },
-"time": 0.733614568318225,
-"velocity": -2.200843704954675
+"time": 0.642128319375051,
+"velocity": -1.926384958125153
 },
 {
-"acceleration": -3.000000000000001,
-"curvature": 0.18001915338928814,
+"acceleration": -2.999999999999998,
+"curvature": 0.3517364776330651,
 "pose": {
 "rotation": {
-"radians": 3.1204277395366393
+"radians": -3.0791339751738596
 },
 "translation": {
-"x": 2.157216157587245,
-"y": 1.5302668918017295
+"x": 1.83548108083196,
+"y": 1.508107238365337
 }
 },
-"time": 0.7787491645618959,
-"velocity": -2.3362474936856876
+"time": 0.6858521936526092,
+"velocity": -2.0575565809578276
 },
 {
-"acceleration": -3.0000000000000058,
-"curvature": 0.14337567393314235,
+"acceleration": -3.0,
+"curvature": 0.275204318591481,
 "pose": {
 "rotation": {
-"radians": 3.102801516143301
+"radians": -3.1085747317854056
 },
 "translation": {
-"x": 2.266826171875,
-"y": 1.5269433593750001
+"x": 1.9299702808260917,
+"y": 1.512564440071583
 }
 },
-"time": 0.8243525871046516,
-"velocity": -2.473057761313955
+"time": 0.7303807753195883,
+"velocity": -2.191142325958765
 },
 {
-"acceleration": -2.9999999999999947,
-"curvature": 0.11472370731530883,
+"acceleration": -3.0,
+"curvature": 0.21684035548350458,
 "pose": {
 "rotation": {
-"radians": 3.0878089519797918
+"radians": -3.133540828236348
 },
 "translation": {
-"x": 2.3835272431652994,
-"y": 1.521505150413141
+"x": 2.032116653909907,
+"y": 1.5146116488520054
 }
 },
-"time": 0.8703116274167206,
-"velocity": -2.610934882250162
+"time": 0.7756077193399169,
+"velocity": -2.3268231580197507
 },
 {
-"acceleration": -2.999999999999993,
-"curvature": 0.09210275028338172,
+"acceleration": -3.000000000000004,
+"curvature": 0.1720386091103925,
 "pose": {
 "rotation": {
-"radians": 3.0750705385459662
+"radians": 3.128442947671593
 },
 "translation": {
-"x": 2.507127021253109,
-"y": 1.5140323826670645
+"x": 2.141845703125,
+"y": 1.514287109375
 }
 },
-"time": 0.9165111255048433,
-"velocity": -2.74953337651453
+"time": 0.8214136313290165,
+"velocity": -2.4642408939870495
 },
 {
-"acceleration": -3.00000000000001,
-"curvature": 0.08258141730627139,
+"acceleration": -3.0,
+"curvature": 0.13735085388913207,
 "pose": {
 "rotation": {
-"radians": 3.0694468510309143
+"radians": 3.1104182045959927
 },
 "translation": {
-"x": 2.5714391557357157,
-"y": 1.5095629048309638
+"x": 2.259019916364923,
+"y": 1.5116502300556744
 }
 },
-"time": 0.9396652663003078,
-"velocity": -2.8189957989009233
+"time": 0.8676729069363213,
+"velocity": -2.603018720808964
 },
 {
-"acceleration": -2.999999999999997,
-"curvature": 0.07405122480858692,
+"acceleration": -3.0000000000000138,
+"curvature": 0.110225734125589,
 "pose": {
 "rotation": {
-"radians": 3.0642743981156304
+"radians": 3.095088114239897
 },
 "translation": {
-"x": 2.637379901343957,
-"y": 1.5046227090526374
+"x": 2.383442504703998,
+"y": 1.5067802855372427
 }
 },
-"time": 0.9628366985989912,
-"velocity": -2.888510095796974
+"time": 0.9142582668283791,
+"velocity": -2.7427748004851376
 },
 {
-"acceleration": -2.999999999999997,
-"curvature": 0.06638630697681559,
+"acceleration": -3.0000000000000098,
+"curvature": 0.09889031559529451,
 "pose": {
 "rotation": {
-"radians": 3.0595229239945767
+"radians": 3.0883075396651454
 },
 "translation": {
-"x": 2.704910826665873,
-"y": 1.4992265377429428
+"x": 2.448295154470543,
+"y": 1.5035378000148922
 }
 },
-"time": 0.9860114737533782,
-"velocity": -2.958034421260135
+"time": 0.9376338750041463,
+"velocity": -2.8129016250124397
 },
 {
-"acceleration": -3.0,
-"curvature": 0.05947762116090556,
+"acceleration": -3.0000000000000098,
+"curvature": 0.08878206680413868,
 "pose": {
 "rotation": {
-"radians": 3.0551650830091903
+"radians": 3.0820590590309878
 },
 "translation": {
-"x": 2.7739906215667727,
-"y": 1.4933900737762453
+"x": 2.514861140726134,
+"y": 1.4997751191724094
 }
 },
-"time": 1.0091758531024695,
-"velocity": -3.027527559307409
+"time": 0.9610439360863878,
+"velocity": -2.8831318082591646
 },
 {
-"acceleration": -2.999999999999997,
-"curvature": 0.05323053392002318,
+"acceleration": -2.999999999999981,
+"curvature": 0.07974131596042953,
 "pose": {
 "rotation": {
-"radians": 3.051176176214879
+"radians": 3.076304757128638
 },
 "translation": {
-"x": 2.844575209611503,
-"y": 1.487129901619337
+"x": 2.5831008579742045,
+"y": 1.4955071619155929
 }
 },
-"time": 1.032316329567719,
-"velocity": -3.0969489887031574
+"time": 0.9844732006884147,
+"velocity": -2.9534196020652455
 },
 {
-"acceleration": -1.6208951747357696,
-"curvature": 0.04756275121984786,
+"acceleration": -2.999999999999994,
+"curvature": 0.07163048889857847,
 "pose": {
 "rotation": {
-"radians": 3.047533912658981
+"radians": 3.071010438641928
 },
 "translation": {
-"x": 2.9166178604867308,
-"y": 1.480463468460366
+"x": 2.652971696853638,
+"y": 1.4907498455047605
 }
 },
-"time": 1.0554196440309045,
-"velocity": -3.166258932092714
+"time": 1.0079066862641157,
+"velocity": -3.0237200587923483
 },
 {
-"acceleration": -0.21630034795556197,
-"curvature": 0.042402546702922254,
+"acceleration": -3.0,
+"curvature": 0.06433083909732847,
 "pose": {
 "rotation": {
-"radians": 3.044218194010263
+"radians": 3.0661452716397024
 },
 "translation": {
-"x": 2.990069302423217,
-"y": 1.4734090453377577
+"x": 2.7244281609615424,
+"y": 1.4855200450072878
 }
 },
-"time": 1.0785871836562022,
-"velocity": -3.203811085281859
+"time": 1.0313296989124234,
+"velocity": -3.093989096737271
 },
 {
-"acceleration": -0.1958473173228695,
-"curvature": 0.0376872463725931,
+"acceleration": -1.2592490394382463,
+"curvature": 0.05773966180426855,
 "pose": {
 "rotation": {
-"radians": 3.0412109209735574
+"radians": 3.0616814662358625
 },
 "translation": {
-"x": 3.0648778346180916,
-"y": 1.4659856882691384
+"x": 2.7974219836760312,
+"y": 1.4798355527501554
 }
 },
-"time": 1.1020331645264878,
-"velocity": -3.208882459102261
+"time": 1.0547278491518168,
+"velocity": -3.1641835474554516
 },
 {
-"acceleration": -0.17816500662631204,
-"curvature": 0.03336193274313943,
+"acceleration": -0.24352731051534277,
+"curvature": 0.051767924660677846,
 "pose": {
 "rotation": {
-"radians": 3.03849581984293
+"radians": 3.0575939861064576
 },
 "translation": {
-"x": 3.1409894396571327,
-"y": 1.4582131993802614
+"x": 2.8719022449789917,
+"y": 1.4737150377724904
 }
 },
-"time": 1.1258582356394338,
-"velocity": -3.213548535364758
+"time": 1.0782357693609725,
+"velocity": -3.193785873398022
 },
 {
-"acceleration": -0.16288220482313998,
-"curvature": 0.029378336125189086,
+"acceleration": -0.21935160660926187,
+"curvature": 0.04633825424722944,
 "pose": {
 "rotation": {
-"radians": 3.0360582875510733
+"radians": 3.0538602903384
 },
 "translation": {
-"x": 3.2183478959370406,
-"y": 1.4501120880339293
+"x": 2.9478154882788656,
+"y": 1.46717800527811
 }
 },
-"time": 1.1500462512785938,
-"velocity": -3.2178579933313864
+"time": 1.1020711195450728,
+"velocity": -3.1995904321235473
 },
 {
-"acceleration": -0.1496847728513606,
-"curvature": 0.02569388502319034,
+"acceleration": -0.19846403649661093,
+"curvature": 0.041383225294409076,
 "pose": {
 "rotation": {
-"radians": 3.0338852536314866
+"radians": 3.050460103039194
 },
 "translation": {
-"x": 3.296894890087715,
-"y": 1.44170353195892
+"x": 3.025105837233423,
+"y": 1.460244756088068
 }
 },
-"time": 1.1745802005498802,
-"velocity": -3.2218541370817126
+"time": 1.1263043114412665,
+"velocity": -3.204906021699248
 },
 {
-"acceleration": -0.13830703558135557,
-"curvature": 0.022270891517827754,
+"acceleration": -0.18041177622586022,
+"curvature": 0.03684390635810752,
 "pose": {
 "rotation": {
-"radians": 3.031965057602426
+"radians": 3.047375208200817
 },
 "translation": {
-"x": 3.376570129394531,
-"y": 1.4330093383789064
+"x": 3.1037151125725355,
+"y": 1.4529363460931926
 }
 },
-"time": 1.1994422605196287,
-"velocity": -3.2255756088809013
+"time": 1.150919122792038,
+"velocity": -3.2097911765175247
 },
 {
-"acceleration": -0.12852453250634654,
-"curvature": 0.01907585095586169,
+"acceleration": -0.16481211425075973,
+"curvature": 0.03266862225664958,
 "pose": {
 "rotation": {
-"radians": 3.03028734039303
+"radians": 3.044589267441331
 },
 "translation": {
-"x": 3.457311454220617,
-"y": 1.4240519051413871
+"x": 3.1835829489209573,
+"y": 1.445274545706634
 }
 },
-"time": 1.2246138476137505,
-"velocity": -3.229057016472767
+"time": 1.1758983837960064,
+"velocity": -3.21429772936406
 },
 {
-"acceleration": -0.12014793463738387,
-"curvature": 0.016078838277770652,
+"acceleration": -0.1513416947427874,
+"curvature": 0.028811899372181933,
 "pose": {
 "rotation": {
-"radians": 3.0288429485525574
+"radians": 3.042087658416093
 },
 "translation": {
-"x": 3.5390549504291267,
-"y": 1.4148541818466036
+"x": 3.264646911621094,
+"y": 1.4372817993164064
 }
 },
-"time": 1.2500756674444913,
-"velocity": -3.232329484963274
+"time": 1.2012240370802936,
+"velocity": -3.218471703826625
 },
 {
-"acceleration": -0.11301795780238003,
-"curvature": 0.013252985906565217,
+"acceleration": -0.13972745325468056,
+"curvature": 0.02523356500716061,
 "pose": {
 "rotation": {
-"radians": 3.0276238501067017
+"radians": 3.0398573318770583
 },
 "translation": {
-"x": 3.621735061805521,
-"y": 1.4054396309764705
+"x": 3.346842613555782,
+"y": 1.428981184737931
 }
 },
-"time": 1.2758077632154612,
-"velocity": -3.2354211431240474
+"time": 1.2268771953069186,
+"velocity": -3.2223540962681474
 },
 {
-"acceleration": -0.10700112528517192,
-"curvature": 0.010574030336617751,
+"acceleration": -0.129739007569556,
+"curvature": 0.02189797638160463,
 "pose": {
 "rotation": {
-"radians": 3.0266230610462874
+"radians": 3.0378866855507565
 },
 "translation": {
-"x": 3.7052847024798394,
-"y": 1.395832189023495
+"x": 3.4301038319710644,
+"y": 1.4203963726665823
 }
 },
-"time": 1.3017895628402554,
-"velocity": -3.2383575530576723
+"time": 1.2528381966565394,
+"velocity": -3.2259815608706712
 },
 {
-"acceleration": -0.10198625426634786,
-"curvature": 0.008019916440438047,
+"acceleration": -0.12118229171655834,
+"curvature": 0.018773358613619326,
 "pose": {
 "rotation": {
-"radians": 3.0258345815506917
+"radians": 3.0361654531949416
 },
 "translation": {
-"x": 3.7896353693489804,
-"y": 1.3860562276197017
+"x": 3.514362625298963,
+"y": 1.4115515861302264
 }
 },
-"time": 1.327999924895309,
-"velocity": -3.2411620912916947
+"time": 1.2790866584078182,
+"velocity": -3.2293870102485096
 },
 {
-"acceleration": -0.09788156025240516,
-"curvature": 0.005570450094773928,
+"acceleration": -0.11389424777925859,
+"curvature": 0.015831234211556567,
 "pose": {
 "rotation": {
-"radians": 3.0252533411597136
+"radians": 3.0346846073756293
 },
 "translation": {
-"x": 3.8747172544989734,
-"y": 1.3761365146655589
+"x": 3.599549449980259,
+"y": 1.4024715599417688
 }
 },
-"time": 1.354417183514112,
-"velocity": -3.243856288546212
+"time": 1.305601528807143,
+"velocity": -3.2326001430080673
 },
 {
-"acceleration": -0.0946122913956068,
-"curvature": 0.0032069910532981387,
+"acceleration": -0.10773841611081388,
+"curvature": 0.013045929290780045,
 "pose": {
 "rotation": {
-"radians": 3.0248751522127137
+"radians": 3.033436274677566
 },
 "translation": {
-"x": 3.9604593576272604,
-"y": 1.3660981754589017
+"x": 3.6855932772872624,
+"y": 1.3931815001516954
 }
 },
-"time": 1.381019192317059,
-"velocity": -3.2464601346736925
+"time": 1.3323611374015032,
+"velocity": -3.2356479084997893
 },
 {
-"acceleration": 0.017153121578660314,
-"curvature": 0.0009121790957482033,
+"acceleration": -0.10260128876078824,
+"curvature": 0.010394143978140808,
 "pose": {
 "rotation": {
-"radians": 3.024696670972615
+"radians": 3.0324136622207964
 },
 "translation": {
-"x": 4.046789598464965,
-"y": 1.3559666538238524
+"x": 3.772421710146591,
+"y": 1.3837070435006176
 }
 },
-"time": 1.407783367459566,
-"velocity": -3.2489923546112385
+"time": 1.359343243987188,
+"velocity": -3.238554917926664
 },
 {
-"acceleration": 0.08990775587557549,
-"curvature": -0.001330312607281367,
+"acceleration": -0.0983893128135574,
+"curvature": 0.007854576343980575,
 "pose": {
 "rotation": {
-"radians": 3.024715365945128
+"radians": 3.0316109945037333
 },
 "translation": {
-"x": 4.133634929199179,
-"y": 1.3457676732397523
+"x": 3.8599610999619474,
+"y": 1.3740742168718132
 }
 },
-"time": 1.434698903071532,
-"velocity": -3.2485306691565317
+"time": 1.3865250863085137,
+"velocity": -3.2413438099797247
 },
 {
-"acceleration": 0.08844869146974021,
-"curvature": -0.003536002232938867,
+"acceleration": -0.09502644982438829,
+"curvature": 0.005407590758830809,
 "pose": {
 "rotation": {
-"radians": 3.024929492990887
+"radians": 3.031023459729339
 },
 "translation": {
-"x": 4.220921446895227,
-"y": 1.3355271979700776
+"x": 3.9481366634368893,
+"y": 1.3643093967437743
 }
 },
-"time": 1.461762861533512,
-"velocity": -3.2460974093861052
+"time": 1.4138834266240485,
+"velocity": -3.2440355782830896
 },
 {
-"acceleration": 0.08766846785336418,
-"curvature": -0.005719806871432327,
+"acceleration": -0.09245221473126805,
+"curvature": 0.0030349228567691806,
 "pose": {
 "rotation": {
-"radians": 3.0253380769108045
+"radians": 3.0306471648965503
 },
 "translation": {
-"x": 4.30857450591895,
-"y": 1.3252713941913683
+"x": 4.036872599397611,
+"y": 1.3544392686427456
 }
 },
-"time": 1.4889597405770916,
-"velocity": -3.24369188102264
+"time": 1.4413945972425684,
+"velocity": -3.2466498671574806
 },
 {
-"acceleration": 0.08754486387052318,
-"curvature": -0.007896237174027189,
+"acceleration": 0.0332454467141077,
+"curvature": 0.0007194143394314367,
 "pose": {
 "rotation": {
-"radians": 3.025940899263675
+"radians": 3.0304790990549484
 },
 "translation": {
-"x": 4.396518830358983,
-"y": 1.3150265911221508
+"x": 4.1260922056157145,
+"y": 1.3444907865952702
 }
 },
-"time": 1.5162655755491454,
-"velocity": -3.241298020307183
+"time": 1.4690345451171203,
+"velocity": -3.2492052415535397
 },
 {
-"acceleration": 0.08806657424409889,
-"curvature": -0.010079582986619252,
+"acceleration": 0.08900861603629157,
+"curvature": -0.0015552282927552242,
 "pose": {
 "rotation": {
-"radians": 3.026738492250795
+"radians": 3.0305171042281356
 },
 "translation": {
-"x": 4.484678626449022,
-"y": 1.3048192421518614
+"x": 4.2157179956309845,
+"y": 1.3344911325807338
 }
 },
-"time": 1.5436563258529152,
-"velocity": -3.238900100800528
+"time": 1.4967935518307562,
+"velocity": -3.248282380975005
 },
 {
-"acceleration": 0.08923275607184701,
-"curvature": -0.012284093622185,
+"acceleration": 0.08816968296671608,
+"curvature": -0.003804656315024218,
 "pose": {
 "rotation": {
-"radians": 3.0277321385760345
+"radians": 3.0307598536115794
 },
 "translation": {
-"x": 4.572977694990113,
-"y": 1.2946758859697738
+"x": 4.305671815574169,
+"y": 1.3244676759839056
 }
 },
-"time": 1.5711079138540933,
-"velocity": -3.236482533487704
+"time": 1.5246683231520937,
+"velocity": -3.2458012861573646
 },
 {
-"acceleration": 0.09105284216376199,
-"curvature": -0.014524156271563539,
+"acceleration": 0.08800388509379588,
+"curvature": -0.006044087360713388,
 "pose": {
 "rotation": {
-"radians": 3.028923877262368
+"radians": 3.031206836745122
 },
 "translation": {
-"x": 4.661339543772921,
-"y": 1.284623107693915
+"x": 4.395874960989749,
+"y": 1.3144479330474859
 }
 },
-"time": 1.5985962644699692,
-"velocity": -3.23402967220238
+"time": 1.5526405927636824,
+"velocity": -3.2433349800138513
 },
 {
-"acceleration": 0.09354660878516664,
-"curvature": -0.016814475989020715,
+"acceleration": 0.08850039194863434,
+"curvature": -0.008288504315879417,
 "pose": {
 "rotation": {
-"radians": 3.0303165154783476
+"radians": 3.0318583514502686
 },
 "translation": {
-"x": 4.7496875,
-"y": 1.2746875000000006
+"x": 4.4862482936587185,
+"y": 1.3044595263246448
 }
 },
-"time": 1.6260973454667302,
-"velocity": -3.231525620615049
+"time": 1.5806852578747532,
+"velocity": -3.2408669405279227
 },
 {
-"acceleration": 0.09674449398929982,
-"curvature": -0.01917026071472533,
+"acceleration": 0.08965932492170785,
+"curvature": -0.010552848551403085,
 "pose": {
 "rotation": {
-"radians": 3.031913646501378
+"radians": 3.032715502409006
 },
 "translation": {
-"x": 4.837944822708087,
-"y": 1.2648956242503493
+"x": 4.576712358421355,
+"y": 1.294530144131568
 }
 },
-"time": 1.6535872084985475,
-"velocity": -3.228954037152454
+"time": 1.6087772105659763,
+"velocity": -3.238380791704147
 },
 {
-"acceleration": 0.1006881708186756,
-"curvature": -0.02160741491112659,
+"acceleration": 0.09149159983450836,
+"curvature": -0.012852211348924768,
 "pose": {
 "rotation": {
-"radians": 3.0337196740199563
+"radians": 3.033780206345614
 },
 "translation": {
-"x": 4.926034815190361,
-"y": 1.2552739716228105
+"x": 4.6671875,
+"y": 1.2846875000000004
 }
 },
-"time": 1.681042030931163,
-"velocity": -3.2262979342486444
+"time": 1.636891379034952,
+"velocity": -3.2358600943384834
 },
 {
-"acceleration": 0.10543138754364552,
-"curvature": -0.02414274559744336,
+"acceleration": 0.09401904279786097,
+"curvature": -0.0152020273119732,
 "pose": {
 "rotation": {
-"radians": 3.035739843055413
+"radians": 3.035055203856956
 },
 "translation": {
-"x": 5.013880937418726,
-"y": 1.2458489242396906
+"x": 4.757593979821832,
+"y": 1.2749592921297879
 }
 },
-"time": 1.7084381585022963,
-"velocity": -3.223539468275992
+"time": 1.6650027696111627,
+"velocity": -3.233288138241093
 },
 {
-"acceleration": 0.11104109505349889,
-"curvature": -0.02679418487197453,
+"acceleration": 0.09727477690135776,
+"curvature": -0.017618273590137814,
 "pose": {
 "rotation": {
-"radians": 3.037980277866127
+"radians": 3.0365440780213637
 },
 "translation": {
-"x": 5.1014069184660915,
-"y": 1.2366467162966739
+"x": 4.847852092841641,
+"y": 1.2653731628414242
 }
 },
-"time": 1.7357521488816365,
-"velocity": -3.2206597163709443
+"time": 1.6930865095772074,
+"velocity": -3.2306477318913016
 },
 {
-"acceleration": 0.11759888976524756,
-"curvature": -0.029581033424534397,
+"acceleration": 0.10130388623782942,
+"curvature": -0.020117678882778878,
 "pose": {
 "rotation": {
-"radians": 3.0404480272849157
+"radians": 3.0382512800028074
 },
 "translation": {
-"x": 5.18853686892864,
-"y": 1.2276933951917464
+"x": 4.937882284364605,
+"y": 1.2559566580285901
 }
 },
-"time": 1.7629608162044181,
-"velocity": -3.2176384361564763
+"time": 1.7211178908435005,
+"velocity": -3.227920985532386
 },
 {
-"acceleration": 0.1252028091606999,
-"curvature": -0.03252423007291641,
+"acceleration": 0.10616437256530456,
+"curvature": -0.022717946433465058,
 "pose": {
 "rotation": {
-"radians": 3.0431511180344772
+"radians": 3.040182161956794
 },
 "translation": {
-"x": 5.275195393348113,
-"y": 1.2190147826541224
+"x": 5.0276052668690685,
+"y": 1.2467371866106989
 }
 },
-"time": 1.7900412766649607,
-"velocity": -3.2144538040719848
+"time": 1.7490724145344305,
+"velocity": -3.2250890836445674
 },
 {
-"acceleration": 0.13396952647048432,
-"curvature": -0.03564665302312524,
+"acceleration": 0.11192842885657366,
+"curvature": -0.025437995584294812,
 "pose": {
 "rotation": {
-"radians": 3.0460986166674964
+"radians": 3.042343017638818
 },
 "translation": {
-"x": 5.361307702634076,
-"y": 1.210636435873167
+"x": 5.116942136829311,
+"y": 1.23774197998544
 }
 },
-"time": 1.8169709952701054,
-"velocity": -3.2110821276527135
+"time": 1.7769258365557792,
+"velocity": -3.2221320425718742
 },
 {
-"acceleration": 0.14403700119980978,
-"curvature": -0.038973459372097,
+"acceleration": 0.11868406335154974,
+"curvature": -0.028298226941188262,
 "pose": {
 "rotation": {
-"radians": 3.049300700888652
+"radians": 3.0447411312164716
 },
 "translation": {
-"x": 5.446799726486208,
-"y": 1.2025836086273198
+"x": 5.205814491538331,
+"y": 1.2289980514813212
 }
 },
-"time": 1.8437278338674323,
-"velocity": -3.2074975266559824
+"time": 1.8046542142259747,
+"velocity": -3.2190284488245076
 },
 {
-"acceleration": 0.15556765315764967,
-"curvature": -0.04253247036650223,
+"acceleration": 0.1265371177434771,
+"curvature": -0.03132081682154602,
 "pose": {
 "rotation": {
-"radians": 3.0527687411362936
+"radians": 3.047384834894003
 },
 "translation": {
-"x": 5.531598225816561,
-"y": 1.1948812124130201
+"x": 5.294144545930613,
+"y": 1.220532155810215
 }
 },
-"time": 1.8702901005796857,
-"velocity": -3.20367157741368
+"time": 1.8322339540677361,
+"velocity": -3.2157551732339087
 },
 {
-"acceleration": 0.1687521393111968,
-"curvature": -0.046354611129002764,
+"acceleration": 0.13561373397767681,
+"curvature": -0.03453004743286898,
 "pose": {
 "rotation": {
-"radians": 3.056515393433181
+"radians": 3.050283576074573
 },
 "translation": {
-"x": 5.615630905171858,
-"y": 1.1875537775736307
+"x": 5.381855249404908,
+"y": 1.2123707485198967
 }
 },
-"time": 1.8966366007951219,
-"velocity": -3.199572914206247
+"time": 1.8596418608720198,
+"velocity": -3.212287055703513
 },
 {
-"acceleration": 0.18381382483475828,
-"curvature": -0.050474414995189336,
+"acceleration": 0.14606333595111157,
+"curvature": -0.037952680187883364,
 "pose": {
 "rotation": {
-"radians": 3.0605547046592134
+"radians": 3.0534479949121365
 },
 "translation": {
-"x": 5.698826525155748,
-"y": 1.180625414428361
+"x": 5.468870402647008,
+"y": 1.2045399454465953
 }
 },
-"time": 1.9227466898837011,
-"velocity": -3.1951667808149433
+"time": 1.8868551881633147,
+"velocity": -3.2085965547755837
 },
 {
-"acceleration": 0.20101405156459667,
-"curvature": -0.05493060430575961,
+"acceleration": 0.1580622051829217,
+"curvature": -0.04161838072616033,
 "pose": {
 "rotation": {
-"radians": 3.064902231555147
+"radians": 3.0568900132431986
 },
 "translation": {
-"x": 5.781115014851095,
-"y": 1.1741197744011886
+"x": 5.555114774452522,
+"y": 1.1970654821675275
 }
 },
-"time": 1.948600327831346,
-"velocity": -3.1904145247378937
+"time": 1.9138516902140001,
+"velocity": -3.2046533556270496
 },
 {
-"acceleration": 0.22065831764270227,
-"curvature": -0.0597667615081691,
+"acceleration": 0.17181774315582565,
+"curvature": -0.045560205616955105,
 "pose": {
 "rotation": {
-"radians": 3.0695751749349918
+"radians": 3.0606229360399007
 },
 "translation": {
-"x": 5.86242758424225,
-"y": 1.1680600111497927
+"x": 5.640514218549652,
+"y": 1.1899726734534486
 }
 },
-"time": 1.9741781360089115,
-"velocity": -3.185273025885979
+"time": 1.940609675776518,
+"velocity": -3.200423929422785
 },
 {
-"acceleration": 0.24310348969758996,
-"curvature": -0.06503210678107174,
+"acceleration": 0.18757352711753633,
+"curvature": -0.049815162401139784,
 "pose": {
 "rotation": {
-"radians": 3.0745925307653774
+"radians": 3.0646615666910972
 },
 "translation": {
-"x": 5.9426968366373325,
-"y": 1.1624687416944655
+"x": 5.724995790421963,
+"y": 1.183286372721195
 }
 },
-"time": 1.9994614563192379,
-"velocity": -3.179694050961881
+"time": 1.9671080637251463,
+"velocity": -3.1958710362081844
 },
 {
-"acceleration": 0.26876617071694464,
-"curvature": -0.07078240114388289,
+"acceleration": 0.2056152799225824,
+"curvature": -0.05442485663489834,
 "pose": {
 "rotation": {
-"radians": 3.079975259960845
+"radians": 3.069022337597961
 },
 "translation": {
-"x": 6.021856881090499,
-"y": 1.1573680075470483
+"x": 5.808487864131167,
+"y": 1.1770309314862244
 }
 },
-"time": 2.024432412994394,
-"velocity": -3.1736235242530633
+"time": 1.9933264408247076,
+"velocity": -3.190953162740322
 },
 {
-"acceleration": 0.2981323386109727,
-"curvature": -0.0770809971882636,
+"acceleration": 0.22627788663999088,
+"curvature": -0.05943624197193476,
 "pose": {
 "rotation": {
-"radians": 3.085746478941121
+"radians": 3.073723457765299
 },
 "translation": {
-"x": 6.099843444824221,
-"y": 1.1527792358398452
+"x": 5.8909202491398895,
+"y": 1.1712301588151606
 }
 },
-"time": 2.049073977345937,
-"velocity": -3.167000705361824
+"time": 2.0192451218716263,
+"velocity": -3.1856238858816357
 },
 {
-"acceleration": 0.3317683460421803,
-"curvature": -0.08400006319085407,
+"acceleration": 0.2499535989384545,
+"curvature": -0.06490249211458578,
 "pose": {
 "rotation": {
-"radians": 3.091931673193759
+"radians": 3.078785079278317
 },
 "translation": {
-"x": 6.1765939856515635,
-"y": 1.148723200454553
+"x": 5.972224307134456,
+"y": 1.1659072807783382
 }
 },
-"time": 2.073370035803015,
-"velocity": -3.1597572646349863
+"time": 2.044845212483379,
+"velocity": -3.179831151480216
 },
 {
-"acceleration": 0.3703333221681629,
-"curvature": -0.09162201043466739,
+"acceleration": 0.27710156916450956,
+"curvature": -0.07088401672704611,
 "pose": {
 "rotation": {
-"radians": 3.098558936273359
+"radians": 3.0842294847746925
 },
 "translation": {
-"x": 6.252047804398464,
-"y": 1.1452199831511836
+"x": 6.052333068847656,
+"y": 1.1610848999023444
 }
 },
-"time": 2.0973054616056337,
-"velocity": -3.151816248004636
+"time": 2.070108674845441,
+"velocity": -3.1735164581411723
 },
 {
-"acceleration": 0.41459292820259114,
-"curvature": -0.10004115803275913,
+"acceleration": 0.3082588441536493,
+"curvature": -0.07744964717884074,
 "pose": {
 "rotation": {
-"radians": 3.105659236831585
+"radians": 3.090081298248968
 },
 "translation": {
-"x": 6.3261461573260025,
-"y": 1.1422889346969907
+"x": 6.131181350881525,
+"y": 1.1567849546225624
 }
 },
-"time": 2.1208661905518253,
-"velocity": -3.1430909249812893
+"time": 2.095018396760084,
+"velocity": -3.1666139351111733
 },
 {
-"acceleration": 0.4654342723588956,
-"curvature": -0.10936567428966636,
+"acceleration": 0.34405291602980925,
+"curvature": -0.08467802230004791,
 "pose": {
 "rotation": {
-"radians": 3.1132667163881784
+"radians": 3.096367721752838
 },
 "translation": {
-"x": 6.398832368552686,
-"y": 1.1399486359953883
+"x": 6.2087058725301185,
+"y": 1.1530286787357182
 }
 },
-"time": 2.144039301225672,
-"velocity": -3.1334835171714563
+"time": 2.119558264378002,
+"velocity": -3.1590493038835903
 },
 {
-"acceleration": 0.5238815598406985,
-"curvature": -0.11971983840973249,
+"acceleration": 0.38521586160024995,
+"curvature": -0.09265920917501876,
 "pose": {
 "rotation": {
-"radians": 3.121419020588816
+"radians": 3.1031188007684074
 },
 "translation": {
-"x": 6.470051942476713,
-"y": 1.1382168592148814
+"x": 6.284845372602285,
+"y": 1.1498365608524175
 }
 },
-"time": 2.166813100154582,
-"velocity": -3.1228838106381316
+"time": 2.1437132390307663,
+"velocity": -3.1507387144176806
 },
 {
-"acceleration": 2.941660058627839,
-"curvature": -0.1312466707318828,
+"acceleration": 0.43259998618029444,
+"curvature": -0.10149659931323658,
 "pose": {
 "rotation": {
-"radians": 3.1301576666037865
+"radians": 3.110367721213085
 },
 "translation": {
-"x": 6.539752676198262,
-"y": 1.1371105289179848
+"x": 6.359540726244451,
+"y": 1.1472283038496975
 }
 },
-"time": 2.1891772123540276,
-"velocity": -3.1111676646546336
+"time": 2.167469438617512,
+"velocity": -3.1415874495255247
 },
 {
-"acceleration": 3.0,
-"curvature": -0.14411098290800894,
+"acceleration": 0.48719469736989557,
+"curvature": -0.1113091261504472,
 "pose": {
 "rotation": {
-"radians": 3.139528449034186
+"radians": 3.1181511411559137
 },
 "translation": {
-"x": 6.6078847719417535,
-"y": 1.136645683190146
+"x": 6.4327350617633785,
+"y": 1.1452227843235594
 }
 },
-"time": 2.21130847982446,
-"velocity": -3.0460649990900532
+"time": 2.1908142240302673,
+"velocity": -3.131488495678585
 },
 {
-"acceleration": 3.0,
-"curvature": -0.1585029003698083,
+"acceleration": 1.9946946417533062,
+"curvature": -0.12223385542576778,
 "pose": {
 "rotation": {
-"radians": -3.1336034210652306
+"radians": 3.1265095603441373
 },
 "translation": {
-"x": 6.67440094947815,
-"y": 1.1368374347686778
+"x": 6.504373877448962,
+"y": 1.1438380120415248
 }
 },
-"time": 2.2333853347940167,
-"velocity": -2.9798344341813827
+"time": 2.21373629112457,
+"velocity": -3.1203209861374837
 },
 {
-"acceleration": 3.0,
-"curvature": -0.17464190620082817,
+"acceleration": 3.000000000000013,
+"curvature": -0.1344290049726349,
 "pose": {
 "rotation": {
-"radians": -3.122811600182203
+"radians": 3.135487730495044
 },
 "translation": {
-"x": 6.739256558547208,
-"y": 1.1376999321716719
+"x": 6.574405158396986,
+"y": 1.143091089395166
 }
 },
-"time": 2.2553959690613437,
-"velocity": -2.9138025313794023
+"time": 2.236344555487554,
+"velocity": -3.0752244023532977
 },
 {
-"acceleration": 3.0,
-"curvature": -0.19278144531511726,
+"acceleration": 3.0000000000000133,
+"curvature": -0.1480774538811026,
 "pose": {
 "rotation": {
-"radians": -3.1112199278792745
+"radians": -3.1380501982643016
 },
 "translation": {
-"x": 6.802409691279758,
-"y": 1.1392463208269348
+"x": 6.642779493331909,
+"y": 1.1429981708526604
 }
 },
-"time": 2.2773237782119673,
-"velocity": -2.8480191039275318
+"time": 2.2588250140001307,
+"velocity": -3.007783026815568
 },
 {
 "acceleration": 3.0,
-"curvature": -0.23627735990549562,
+"curvature": -0.16339080126343422,
 "pose": {
 "rotation": {
-"radians": -3.0853590491648175
+"radians": -3.127678949934291
 },
 "translation": {
-"x": 6.923455282747749,
-"y": 1.1444381049275414
+"x": 6.709450191429643,
+"y": 1.1435744224113291
 }
 },
-"time": 2.3208629416429933,
-"velocity": -2.717401613634454
+"time": 2.2812425238113905,
+"velocity": -2.9405304973817876
 },
 {
-"acceleration": 3.0,
-"curvature": -0.2919074537205893,
+"acceleration": 2.9999999999999964,
+"curvature": -0.18061402950992259,
 "pose": {
 "rotation": {
-"radians": -3.055392179387659
+"radians": -3.1165234213609563
 },
 "translation": {
-"x": 7.037261580796916,
-"y": 1.1524964115861813
+"x": 6.7743733991403134,
+"y": 1.1448339810501782
 }
 },
-"time": 2.3638693027655573,
-"velocity": -2.588382530266762
+"time": 2.303579943987179,
+"velocity": -2.873518236854421
 },
 {
-"acceleration": 2.9999999999999956,
-"curvature": -0.36350880813786735,
+"acceleration": 3.0,
+"curvature": -0.2219694764808598,
 "pose": {
 "rotation": {
-"radians": -3.020561074591319
+"radians": -3.0915855692193515
 },
 "translation": {
-"x": 7.143603515625004,
-"y": 1.1634863281250016
+"x": 6.898816816508771,
+"y": 1.149454179108143
 }
 },
-"time": 2.4062114156716468,
-"velocity": -2.461356191548493
+"time": 2.3479441769175926,
+"velocity": -2.7404255380631812
 },
 {
 "acceleration": 3.0,
-"curvature": -0.45602008353565504,
+"curvature": -0.2749889499662907,
 "pose": {
 "rotation": {
-"radians": -2.979961519060939
+"radians": -3.062615129935622
 },
 "translation": {
-"x": 7.2423106882255555,
-"y": 1.1774531607981782
+"x": 7.015820101788269,
+"y": 1.156949739689006
 }
 },
-"time": 2.4477659804255842,
-"velocity": -2.336692497286681
+"time": 2.391778732366599,
+"velocity": -2.608921871716163
 },
 {
-"acceleration": 2.9999999999999947,
-"curvature": -0.5755332131696194,
+"acceleration": 3.0,
+"curvature": -0.3434514729820416,
 "pose": {
 "rotation": {
-"radians": -2.93253741012421
+"radians": -3.0288580314604197
 },
 "translation": {
-"x": 7.33327096790076,
-"y": 1.1944211909174927
+"x": 7.125146484374999,
+"y": 1.1673925781249999
 }
 },
-"time": 2.4884256726719136,
-"velocity": -2.214713420547693
+"time": 2.4349456329944132,
+"velocity": -2.4794211698327198
 },
 {
-"acceleration": 3.0000000000000058,
-"curvature": -0.7289646320388383,
+"acceleration": 3.0,
+"curvature": -0.4322864443548591,
 "pose": {
 "rotation": {
-"radians": -2.8770998797896867
+"radians": -2.9894082572246337
 },
 "translation": {
-"x": 7.416434089774271,
-"y": 1.2143924309778962
+"x": 7.226615805039185,
+"y": 1.180834252899512
 }
 },
-"time": 2.5281101756977282,
-"velocity": -2.095659911470249
+"time": 2.477313724506175,
+"velocity": -2.3523168952974345
 },
 {
-"acceleration": 3.000000000000003,
-"curvature": -0.9228662121398744,
+"acceleration": 3.0,
+"curvature": -0.5476944877117405,
 "pose": {
 "rotation": {
-"radians": -2.81239687895411
+"radians": -2.943196876442041
 },
 "translation": {
-"x": 7.49181525230408,
-"y": 1.2373453807830836
+"x": 7.3201082542538645,
+"y": 1.1973046681284902
 }
 },
-"time": 2.566781232692969,
-"velocity": -1.9796467404845262
+"time": 2.518766312514753,
+"velocity": -2.2279591312717004
 },
 {
-"acceleration": 2.9999999999999964,
-"curvature": -1.16063294098844,
+"acceleration": 3.0000000000000053,
+"curvature": -0.6969407880393239,
 "pose": {
 "rotation": {
-"radians": -2.7372718465806782
+"radians": -2.88900330175852
 },
 "translation": {
-"x": 7.559498714795343,
-"y": 1.2632337835710512
+"x": 7.405568110523746,
+"y": 1.2168107760418208
 }
 },
-"time": 2.6044623930137023,
-"velocity": -1.8666032595223263
+"time": 2.5592120751446217,
+"velocity": -2.1066218433820945
 },
 {
-"acceleration": 2.999999999999993,
-"curvature": -1.4373874694266848,
+"acceleration": 3.0,
+"curvature": -0.8873512622917717,
 "pose": {
 "rotation": {
-"radians": -2.650957309320111
+"radians": -2.8255137604613862
 },
 "translation": {
-"x": 7.619641394913202,
-"y": 1.2919853821396838
+"x": 7.483007478713991,
+"y": 1.239335279464722
 }
 },
-"time": 2.6412636264404568,
-"velocity": -1.7561995592420632
+"time": 2.5986001641675696,
+"velocity": -1.9884575763132508
 },
 {
 "acceleration": 3.0,
-"curvature": -1.5848634029588624,
+"curvature": -1.1237277561085663,
 "pose": {
 "rotation": {
-"radians": -2.6035793480217784
+"radians": -2.751466826922218
 },
 "translation": {
-"x": 7.646954731198209,
-"y": 1.307404883861019
+"x": 7.552510028379037,
+"y": 1.264835334299132
 }
 },
-"time": 2.6594044413554347,
-"velocity": -1.7017771144971288
+"time": 2.636940314486243,
+"velocity": -1.8734371253572304
 },
 {
 "acceleration": 3.0,
-"curvature": -1.732944009464002,
+"curvature": -1.4032839618710011,
 "pose": {
 "rotation": {
-"radians": -2.553528288811088
+"radians": -2.665937321118641
 },
 "translation": {
-"x": 7.672476466195665,
-"y": 1.3235006749723137
+"x": 7.61423473209143,
+"y": 1.2932412520050995
 }
 },
-"time": 2.6774210710924775,
-"velocity": -1.647727225286
+"time": 2.6743283280354033,
+"velocity": -1.7612730847097493
 },
 {
-"acceleration": 3.000000000000008,
-"curvature": -1.875799384988843,
+"acceleration": 3.000000000000007,
+"curvature": -1.5544705936373298,
 "pose": {
 "rotation": {
-"radians": -2.5010369461947777
+"radians": -2.6187791899050774
 },
 "translation": {
-"x": 7.6962482517949,
-"y": 1.3402558567866798
+"x": 7.642251581292659,
+"y": 1.3085044721778818
 }
 },
-"time": 2.695364686626383,
-"velocity": -1.593896378684283
+"time": 2.6927313248934683,
+"velocity": -1.7060640941355538
 },
 {
-"acceleration": 3.0,
-"curvature": -2.006399663329765,
+"acceleration": 2.9999999999999925,
+"curvature": -1.708115004022341,
 "pose": {
 "rotation": {
-"radians": -2.4464566592897903
+"radians": -2.5687961319840333
 },
 "translation": {
-"x": 7.718316955566408,
-"y": 1.3576516723632812
+"x": 7.668419603770599,
+"y": 1.32445520208217
 }
 },
-"time": 2.7132974372196244,
-"velocity": -1.5400981269045582
+"time": 2.7109874717334463,
+"velocity": -1.6512956536156203
 },
 {
-"acceleration": 3.0,
-"curvature": -2.1169822256665207,
+"acceleration": 2.999999999999992,
+"curvature": -1.8584641587924655,
+"pose": {
+"rotation": {
+"radians": -2.5161948842551274
+},
+"translation": {
+"x": 7.692781514795208,
+"y": 1.3410766028499328
+}
+},
+"time": 2.729146892376763,
+"velocity": -1.5968173916856716
+},
+{
+"acceleration": 3.000000000000008,
+"curvature": -1.9983547540854674,
 "pose": {
 "rotation": {
-"radians": -2.390263156762404
+"radians": -2.461305410754052
 },
 "translation": {
-"x": 7.7387347731841185,
-"y": 1.375667467636231
+"x": 7.71538543701172,
+"y": 1.3583499145507787
 }
 },
-"time": 2.7312933501923613,
-"velocity": -1.4861103879863469
+"time": 2.747271073896608,
+"velocity": -1.5424448471261356
 },
 {
 "acceleration": 3.0,
-"curvature": -2.199787085492846,
+"curvature": -2.119631110195264,
 "pose": {
 "rotation": {
-"radians": -2.3330520699471444
+"radians": -2.4045902034103666
 },
 "translation": {
-"x": 7.75755934084766,
-"y": 1.3942806525435287
+"x": 7.736285017263438,
+"y": 1.3762544156445067
 }
 },
-"time": 2.749439257683885,
-"velocity": -1.4316726655117757
+"time": 2.7654338428253578,
+"velocity": -1.4879565403398858
 },
 {
 "acceleration": 3.0000000000000044,
-"curvature": -2.247965188475603,
+"curvature": -2.213877649938844,
 "pose": {
 "rotation": {
-"radians": -2.2755223389732424
+"radians": -2.3466430093591018
 },
 "translation": {
-"x": 7.774853847704668,
-"y": 1.4134666621559902
+"x": 7.75553954341449,
+"y": 1.3947673824336366
 }
 },
-"time": 2.7678358682943016,
-"velocity": -1.3764828336805266
+"time": 2.7837223486966036,
+"velocity": -1.433091022726148
 },
 {
 "acceleration": 3.0000000000000044,
-"curvature": -2.2564781741219013,
+"curvature": -2.2733889438365678,
 "pose": {
 "rotation": {
-"radians": -2.218447901053217
+"radians": -2.2881744755849964
 },
 "translation": {
-"x": 7.790687148272994,
-"y": 1.4331989178061493
+"x": 7.773214061172621,
+"y": 1.4138640485159666
 }
 },
-"time": 2.7865992034414235,
-"velocity": -1.3201928282391608
+"time": 2.8022381617663705,
+"velocity": -1.3775435835168477
 },
 {
-"acceleration": 3.0000000000000044,
-"curvature": -2.222768702870628,
+"acceleration": 3.0,
+"curvature": -2.2921938853314927,
 "pose": {
 "rotation": {
-"radians": -2.1626406287471385
+"radians": -2.2299843969592845
 },
 "translation": {
-"x": 7.805133874863104,
-"y": 1.4534487882172105
+"x": 7.7893794909119665,
+"y": 1.4335175642371176
 }
 },
-"time": 2.8058627666317597,
-"velocity": -1.2624021386681517
+"time": 2.821098696199041,
+"velocity": -1.3209619802188355
 },
 {
-"acceleration": 3.0000000000000044,
-"curvature": -2.147019887851784,
+"acceleration": 2.9999999999999956,
+"curvature": -2.2668848355674553,
 "pose": {
 "rotation": {
-"radians": -2.1089096842820165
+"radians": -2.1729231580530635
 },
 "translation": {
-"x": 7.818274550000214,
-"y": 1.4741855506319528
+"x": 7.804112744495794,
+"y": 1.453698956143052
 }
 },
-"time": 2.8257810353021497,
-"velocity": -1.2026473326569813
+"time": 2.8404393234692606,
+"velocity": -1.2629400984081767
+},
+{
+"acceleration": 3.0,
+"curvature": -2.197023554160079,
+"pose": {
+"rotation": {
+"radians": -2.1178476864848435
+},
+"translation": {
+"x": 7.817496842099357,
+"y": 1.47437708643265
+}
+},
+"time": 2.860416768951372,
+"velocity": -1.2030077619618424
 },
 {
 "acceleration": 2.9999999999999956,
-"curvature": -2.0319352230717658,
+"curvature": -2.0850101781568213,
 "pose": {
 "rotation": {
-"radians": -2.0580234574011715
+"radians": -2.0655787716459524
 },
 "translation": {
-"x": 7.830195698846663,
-"y": 1.495376351941668
+"x": 7.8296210290325705,
+"y": 1.4955186124102315
 }
 },
-"time": 2.846535220594216,
-"velocity": -1.1403847767807822
+"time": 2.8812147470045146,
+"velocity": -1.1406138278024156
 },
 {
-"acceleration": 3.000000000000002,
-"curvature": -1.8821100585269588,
+"acceleration": 2.9999999999999987,
+"curvature": -1.9354634974970457,
 "pose": {
 "rotation": {
-"radians": -2.0106796589838902
+"radians": -2.0168663213229143
 },
 "translation": {
-"x": 7.840989961624159,
-"y": 1.5169861698150635
+"x": 7.840580892562866,
+"y": 1.5170879459381084
 }
 },
-"time": 2.8683428705600456,
-"velocity": -1.0749618268832932
+"time": 2.9030534245564934,
+"velocity": -1.0750977951464797
 },
 {
 "acceleration": 3.0000000000000013,
-"curvature": -1.5008887561449302,
+"curvature": -1.5477251501503089,
 "pose": {
 "rotation": {
-"radians": -1.9289608374840002
+"radians": -1.9326369749201937
 },
 "translation": {
-"x": 7.859599639689558,
-"y": 1.56131168658845
+"x": 7.859422409208489,
+"y": 1.5613562125992004
 }
 },
-"time": 2.9162692327809165,
-"velocity": -0.9311827402206804
+"time": 2.95101309832146,
+"velocity": -0.9312187738515799
 },
 {
-"acceleration": 2.9999999999999996,
-"curvature": -1.045883872814297,
+"acceleration": 3.0,
+"curvature": -1.079877466874532,
 "pose": {
 "rotation": {
-"radians": -1.867541251036561
+"radians": -1.869242215241588
 },
 "translation": {
-"x": 7.874971279203904,
-"y": 1.6068370607495304
+"x": 7.874917368590833,
+"y": 1.6068507316708533
 }
 },
-"time": 2.973067449556289,
-"velocity": -0.7607880898945626
+"time": 3.0078222739010383,
+"velocity": -0.7607912471128448
 },
 {
-"acceleration": 2.9999999999999996,
-"curvature": 1.9463370526035135e-14,
+"acceleration": 3.0,
+"curvature": 1.4417311500766886e-15,
 "pose": {
 "rotation": {
-"radians": -1.815774989921777
+"radians": -1.8157749899217621
 },
 "translation": {
-"x": 7.900000000000012,
-"y": 1.6999999999999993
+"x": 7.900000000000003,
+"y": 1.6999999999999975
 }
 },
-"time": 3.226663479521143,
+"time": 3.2614193562719866,
 "velocity": -0.0
 }
 ]
\ No newline at end of file
diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java
index 0fdc741c..6d9bb3f6 100644
--- a/src/main/java/com/stuypulse/robot/Robot.java
+++ b/src/main/java/com/stuypulse/robot/Robot.java
@@ -6,8 +6,8 @@
 package com.stuypulse.robot;
 
 import com.stuypulse.robot.commands.*;
+import com.stuypulse.robot.util.Target;
 
-import edu.wpi.first.util.net.PortForwarder;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
@@ -26,8 +26,9 @@ public class Robot extends TimedRobot {
     @Override
     public void robotInit() {
         robot = new RobotContainer();
-        
+
         DataLogManager.start();
+        Target.forward();
     }
 
     @Override
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
index cf202bc2..359b6c89 100644
--- a/src/main/java/com/stuypulse/robot/RobotContainer.java
+++ b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -142,6 +142,7 @@ public void configureAutons() {
     }
 
     public Command getAutonomousCommand() {
-        return autonChooser.getSelected();
+        return new FiveBallAuton(this);
+        // return autonChooser.getSelected();
     }
 }
diff --git a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainAlignCommand.java b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainAlignCommand.java
index df5fe859..81378e4e 100644
--- a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainAlignCommand.java
+++ b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainAlignCommand.java
@@ -33,7 +33,8 @@ public class DrivetrainAlignCommand extends CommandBase {
     protected final Controller angleController;
     protected final Controller distanceController;
 
-    public DrivetrainAlignCommand(Drivetrain drivetrain, Number targetDistance, double debounceTime) {
+    public DrivetrainAlignCommand(
+            Drivetrain drivetrain, Number targetDistance, double debounceTime) {
         this.drivetrain = drivetrain;
 
         // find errors
diff --git a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainAlignToShootCommand.java b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainAlignToShootCommand.java
index 91fa8d41..aac8ce2b 100644
--- a/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainAlignToShootCommand.java
+++ b/src/main/java/com/stuypulse/robot/commands/drivetrain/DrivetrainAlignToShootCommand.java
@@ -21,7 +21,7 @@ public class DrivetrainAlignToShootCommand extends DrivetrainAlignCommand {
 
     public DrivetrainAlignToShootCommand(
             Drivetrain drivetrain, Conveyor conveyor, Number distance) {
-        super(drivetrain, distance, 1.0 / 3.0);
+        super(drivetrain, distance);
 
         this.conveyor = conveyor;
         this.emptied = new Debouncer(Settings.Conveyor.DEBOUNCE_TIME, DebounceType.kRising);
diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
index 3e06c252..5b478813 100644
--- a/src/main/java/com/stuypulse/robot/constants/Settings.java
+++ b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -87,7 +87,7 @@ public interface ColorSensor {
         SmartNumber TARGET_BIAS = new SmartNumber("Color Sensor/Target Bias", 1.5);
 
         // How long it takes to accept / reject balls
-        double DEBOUNCE_TIME = 1.0 / 8.0;
+        double DEBOUNCE_TIME = 1.0 / 6.0;
 
         public interface BallRGB {
             Color RED = new Color(0.42, 0.39, 0.19);
@@ -217,6 +217,8 @@ public interface Intake {
     }
 
     public interface LED {
+        SmartBoolean SWAP_RAINBOW = new SmartBoolean("Swap Rainbow", false);
+
         double MANUAL_UPDATE_TIME = 0.75;
 
         double BLINK_TIME = 0.5;
@@ -224,6 +226,10 @@ public interface LED {
         double DEBOUNCE_TIME = 0.75;
 
         double RPM_ERROR_STEP = 50;
+
+        double MIN_MATCH_TIME = 1; // non-game modes return 0 for remaning match time
+        double CLIMB_TIME = 45;
+        double END_GAME_TIME = 20;
     }
 
     public interface Shooter {
@@ -234,7 +240,7 @@ public interface Shooter {
         SmartNumber RING_RPM = new SmartNumber("Shooter/Ring RPM", 2950);
         SmartNumber FENDER_RPM = new SmartNumber("Shooter/Fender RPM", 2575);
         SmartNumber FEEDER_MULTIPLER = new SmartNumber("Shooter/Feeder Multipler", 0.9);
-        
+
         double INTEGRAL_MAX_RPM_ERROR = 500;
         double INTEGRAL_MAX_ADJUST = 1.0;
 
@@ -296,7 +302,7 @@ public interface Limelight {
         // if the intake is on the ring, distance of limelight to hub
         double CENTER_TO_HUB = Field.Hub.UPPER_RADIUS;
         double LIMELIGHT_TO_INTAKE = Units.inchesToMeters(30);
-        double RING_SHOT_DISTANCE = Units.inchesToMeters(145) - CENTER_TO_HUB - LIMELIGHT_TO_INTAKE;
+        double RING_SHOT_DISTANCE = Units.inchesToMeters(140) - CENTER_TO_HUB - LIMELIGHT_TO_INTAKE;
 
         double HEIGHT_DIFFERENCE = Field.Hub.HEIGHT - LIMELIGHT_HEIGHT;
 
diff --git a/src/main/java/com/stuypulse/robot/subsystems/ColorSensor.java b/src/main/java/com/stuypulse/robot/subsystems/ColorSensor.java
index 8f90acec..ffa5c8dd 100644
--- a/src/main/java/com/stuypulse/robot/subsystems/ColorSensor.java
+++ b/src/main/java/com/stuypulse/robot/subsystems/ColorSensor.java
@@ -55,10 +55,10 @@ public Sensor() {
 
         public void update() {
             this.connected = Settings.ColorSensor.ENABLED.get();
-            if (this.connected) this.connected &= DriverStation.isTeleopEnabled();
+            if (this.connected) this.connected &= !DriverStation.isAutonomous();
             if (this.connected) this.connected &= colorSensor.isConnected();
 
-            if (this.connected || DriverStation.isDisabled()) this.color = colorSensor.getColor();
+            if (this.connected) this.color = colorSensor.getColor();
             else this.color = Color.kBlack;
         }
     }
diff --git a/src/main/java/com/stuypulse/robot/subsystems/LEDController.java b/src/main/java/com/stuypulse/robot/subsystems/LEDController.java
index 1e68737b..395e40cf 100644
--- a/src/main/java/com/stuypulse/robot/subsystems/LEDController.java
+++ b/src/main/java/com/stuypulse/robot/subsystems/LEDController.java
@@ -77,9 +77,22 @@ public LEDColor getDefaultColor() {
          * finished green .75 second - Pick up a ball and sensed in the color sensor flash color of
          * ball .75 second blue/orange - Two correct ball green
          */
-        if (robot.pump.getCompressing()) return LEDColor.HEARTBEAT;
+        if (DriverStation.isTest() && robot.pump.getCompressing()) return LEDColor.HEARTBEAT;
+
+        // time based LEDs
+        double time = DriverStation.getMatchTime(); // time remaining in a game
+        if (time > Settings.LED.MIN_MATCH_TIME) { 
+            if (time < Settings.LED.END_GAME_TIME) 
+                return LEDColor.RED;
+            if (time < Settings.LED.CLIMB_TIME) 
+                return LEDColor.RAINBOW.pulse();
+        }
 
-        if (robot.conveyor.isFull()) return LEDColor.RAINBOW;
+        if (Settings.LED.SWAP_RAINBOW.get()) {
+            if (!robot.conveyor.isFull()) return LEDColor.RAINBOW;
+        } else {
+            if (robot.conveyor.isFull()) return LEDColor.RAINBOW;
+        }
 
         double shooterError =
                 Math.abs(robot.shooter.getRawTargetRPM() - robot.shooter.getShooterRPM());
diff --git a/src/main/java/com/stuypulse/robot/util/Target.java b/src/main/java/com/stuypulse/robot/util/Target.java
index a5a2cbf9..cb66ecb8 100644
--- a/src/main/java/com/stuypulse/robot/util/Target.java
+++ b/src/main/java/com/stuypulse/robot/util/Target.java
@@ -11,10 +11,22 @@
 
 import com.stuypulse.robot.constants.Settings;
 
+import edu.wpi.first.util.net.PortForwarder;
+
 public class Target {
 
     private static final Limelight kLimelight = Limelight.getInstance();
 
+    private static void forward(int... ports) {
+        for (int port : ports) {
+            PortForwarder.add(port, "limelight.local", port);
+        }
+    }
+
+    public static void forward() {
+        forward(5800, 5801, 5802, 5803, 5804, 5805);
+    }
+
     public static void enable() {
         kLimelight.setLEDMode(LEDMode.FORCE_ON);
     }