diff --git a/build.gradle b/build.gradle
index df2f92a4..a38de3bf 100644
--- a/build.gradle
+++ b/build.gradle
@@ -54,6 +54,8 @@ dependencies {
     testImplementation 'org.junit.jupiter:junit-jupiter-api:5.8.2'
     testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.8.2'
 
+    implementation 'org.apache.commons:commons-math3:3.6.1'
+
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()
 
diff --git a/limelight/Intake blue.vpr b/limelight/Intake blue.vpr
new file mode 100644
index 00000000..f595568b
--- /dev/null
+++ b/limelight/Intake blue.vpr	
@@ -0,0 +1,68 @@
+area_max:100
+area_min:7.311616000000002
+area_similarity:0
+aspect_max:3.794947199999998
+aspect_min:0.3001249999999999
+black_level:0
+blue_balance:2500
+calibration_type:0
+contour_grouping:0
+contour_sort_final:0
+convexity_max:100
+convexity_min:77.2
+corner_approx:5.000000
+crop_x_max:1
+crop_x_min:-1
+crop_y_max:0.13
+crop_y_min:-1
+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:0
+direction_filter:0
+dual_close_sort_origin:0
+erosion_steps:1
+exposure:1200
+force_convex:1
+hue_max:129
+hue_min:99
+image_flip:1
+image_source:0
+img_to_show:0
+intersection_filter:0
+invert_hue:0
+multigroup_max:7
+multigroup_min:1
+multigroup_rejector:0
+pipeline_led_enabled:0
+pipeline_led_power:100
+pipeline_res:0
+pipeline_type:0
+red_balance:1988
+roi_x:0.000000
+roi_y:0.000000
+sat_max:255
+sat_min:107
+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:43
+x_outlier_miqr:1.5
+y_max:1.000000
+y_min:-1.000000
+y_outlier_miqr:1.5
diff --git a/limelight/Intake red.vpr b/limelight/Intake red.vpr
new file mode 100644
index 00000000..f5540c46
--- /dev/null
+++ b/limelight/Intake red.vpr	
@@ -0,0 +1,68 @@
+area_max:100
+area_min:7.311616000000002
+area_similarity:0
+aspect_max:3.794947199999998
+aspect_min:0.3001249999999999
+black_level:0
+blue_balance:2500
+calibration_type:0
+contour_grouping:0
+contour_sort_final:0
+convexity_max:100
+convexity_min:77.2
+corner_approx:5.000000
+crop_x_max:1
+crop_x_min:-1
+crop_y_max:0.13
+crop_y_min:-1
+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:0
+direction_filter:0
+dual_close_sort_origin:0
+erosion_steps:0
+exposure:1200
+force_convex:1
+hue_max:175
+hue_min:154
+image_flip:1
+image_source:0
+img_to_show:0
+intersection_filter:0
+invert_hue:0
+multigroup_max:7
+multigroup_min:1
+multigroup_rejector:0
+pipeline_led_enabled:0
+pipeline_led_power:100
+pipeline_res:0
+pipeline_type:0
+red_balance:1646
+roi_x:0.000000
+roi_y:0.000000
+sat_max:255
+sat_min:202
+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:132
+x_outlier_miqr:1.5
+y_max:1.000000
+y_min:-1.000000
+y_outlier_miqr:1.5
diff --git a/limelight/Shooter.vpr b/limelight/Shooter.vpr
new file mode 100644
index 00000000..8c796f7a
--- /dev/null
+++ b/limelight/Shooter.vpr
@@ -0,0 +1,68 @@
+area_max:100
+area_min:0.0017850625000000004
+area_similarity:15
+aspect_max:20.000000
+aspect_min:0.000000
+black_level:5
+blue_balance:1331
+calibration_type:0
+contour_grouping:3
+contour_sort_final:0
+convexity_max:100
+convexity_min:10
+corner_approx:5.000000
+crop_x_max:1.000000
+crop_x_min:-1.000000
+crop_y_max:1.000000
+crop_y_min:-1.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:0
+direction_filter:0
+dual_close_sort_origin:0
+erosion_steps:0
+exposure:2
+force_convex:1
+hue_max:89
+hue_min:55
+image_flip:1
+image_source:0
+img_to_show:0
+intersection_filter:0
+invert_hue:0
+multigroup_max:5
+multigroup_min:3
+multigroup_rejector:1
+pipeline_led_enabled:0
+pipeline_led_power:100
+pipeline_res:0
+pipeline_type:0
+red_balance:1599
+roi_x:0.000000
+roi_y:0.000000
+sat_max:255
+sat_min:81
+send_corners:1
+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:85
+x_outlier_miqr:0.7
+y_max:1.000000
+y_min:-1.000000
+y_outlier_miqr:1.6
diff --git a/src/main/deploy/autos/blue/5ball.json b/src/main/deploy/autos/blue/5ball.json
index 0b3e67af..c9be3e67 100644
--- a/src/main/deploy/autos/blue/5ball.json
+++ b/src/main/deploy/autos/blue/5ball.json
@@ -212,7 +212,7 @@
     "constraints" : [ ]
   }, {
     "type" : "script",
-    "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 7",
+    "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 5",
     "closed" : false,
     "valid" : true,
     "sendableScript" : {
@@ -220,7 +220,7 @@
       "delay" : 0.0,
       "commands" : [ {
         "methodName" : "frc.subsystem.VisionManager.shootBalls",
-        "args" : [ "7" ],
+        "args" : [ "5" ],
         "argTypes" : [ "double" ],
         "reflection" : true
       } ]
@@ -257,445 +257,459 @@
           "y" : -3.196845054626465
         },
         "rotation" : {
-          "radians" : 2.70041390445855
+          "radians" : -3.128825218368557
         }
       },
-      "curvature" : -0.0
+      "curvature" : 0.0
     }, {
-      "time" : 0.23664586321986603,
-      "velocity" : 0.9465834528794639,
+      "time" : 0.18070546442899316,
+      "velocity" : 0.7228218577159725,
       "acceleration" : 4.000000000000001,
       "pose" : {
         "translation" : {
-          "x" : 7.52540296068446,
-          "y" : -3.1488323883245357
+          "x" : 7.561287415918969,
+          "y" : -3.1975411300314462
         },
         "rotation" : {
-          "radians" : 2.6953045863269516
+          "radians" : -3.1350447706508953
         }
       },
-      "curvature" : -0.07577684722310894
+      "curvature" : -0.18371794738756367
     }, {
-      "time" : 0.34030220571431735,
-      "velocity" : 1.3612088228572694,
-      "acceleration" : 4.0000000000000036,
+      "time" : 0.2560291531859158,
+      "velocity" : 1.0241166127436632,
+      "acceleration" : 4.000000000000002,
       "pose" : {
         "translation" : {
-          "x" : 7.417757449569763,
-          "y" : -3.0966914842865663
+          "x" : 7.495494536036858,
+          "y" : -3.197464238592829
         },
         "rotation" : {
-          "radians" : 2.6858123918336316
+          "radians" : 3.1311609020207034
         }
       },
-      "curvature" : -0.07326544503040545
+      "curvature" : -0.3247341642611955
     }, {
-      "time" : 0.384629630154288,
-      "velocity" : 1.5385185206171523,
-      "acceleration" : 4.0000000000000036,
+      "time" : 0.31447963957810243,
+      "velocity" : 1.2579185583124097,
+      "acceleration" : 3.999999999999992,
       "pose" : {
         "translation" : {
-          "x" : 7.360111402026817,
-          "y" : -3.0682766663898775
+          "x" : 7.428818325054408,
+          "y" : -3.195966399958145
         },
         "rotation" : {
-          "radians" : 2.6816220169257763
+          "radians" : 3.106029813407485
         }
       },
-      "curvature" : -0.056343210130415364
+      "curvature" : -0.4213902848253676
     }, {
-      "time" : 0.42640049414944053,
-      "velocity" : 1.7056019765977626,
+      "time" : 0.3645154779554353,
+      "velocity" : 1.4580619118217408,
       "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 7.299449204304892,
-          "y" : -3.0380968957629193
+          "x" : 7.360957192140631,
+          "y" : -3.192528116363974
         },
         "rotation" : {
-          "radians" : 2.6784957933752187
+          "radians" : 3.0752789658716972
         }
       },
-      "curvature" : -0.035816849566663374
+      "curvature" : -0.47718431116456617
     }, {
-      "time" : 0.4664026360786243,
-      "velocity" : 1.8656105443144977,
-      "acceleration" : 3.9999999999999942,
+      "time" : 0.409416754303078,
+      "velocity" : 1.6376670172123116,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 7.235577081185269,
-          "y" : -3.0061231284680474
+          "x" : 7.2916967297335304,
+          "y" : -3.1867497196945394
         },
         "rotation" : {
-          "radians" : 2.6767024131530928
+          "radians" : 3.041183989047963
         }
       },
-      "curvature" : -0.014633323030462457
+      "curvature" : -0.498764188481344
     }, {
-      "time" : 0.5050798369132412,
-      "velocity" : 2.0203193476529653,
+      "time" : 0.45085224980311017,
+      "velocity" : 1.8034089992124402,
       "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 7.168413784820586,
-          "y" : -2.972412885632366
+          "x" : 7.22090281623241,
+          "y" : -3.178342718540307
         },
         "rotation" : {
-          "radians" : 2.676381564257267
+          "radians" : 3.005659202130603
         }
       },
-      "curvature" : 0.005723704793670682
+      "curvature" : -0.49412155680179287
     }, {
-      "time" : 0.5426791778444782,
-      "velocity" : 2.170716711377913,
-      "acceleration" : 3.9999999999999947,
+      "time" : 0.48978986213967873,
+      "velocity" : 1.9591594485587145,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 7.097979906595825,
-          "y" : -2.9371007325542138
+          "x" : 7.148514718690194,
+          "y" : -3.16712114525658
         },
         "rotation" : {
-          "radians" : 2.6775963537201526
+          "radians" : 2.970217325515413
         }
       },
-      "curvature" : 0.024736031322158278
+      "curvature" : -0.4711532022444734
     }, {
-      "time" : 0.579328420101757,
-      "velocity" : 2.3173136804070285,
-      "acceleration" : 4.0,
+      "time" : 0.5268309899893706,
+      "velocity" : 2.107323959957482,
+      "acceleration" : 3.9999999999999885,
       "pose" : {
         "translation" : {
-          "x" : 7.024387188989294,
-          "y" : -2.900388757809651
+          "x" : 7.074538195505738,
+          "y" : -3.1529929030220956
         },
         "rotation" : {
-          "radians" : 2.6803717421830306
+          "radians" : 2.9359808911961505
         }
       },
-      "curvature" : 0.04244185336482878
+      "curvature" : -0.4367169763361537
     }, {
-      "time" : 0.6150805872232928,
-      "velocity" : 2.460322348893172,
-      "acceleration" : 4.000000000000005,
+      "time" : 0.5623614610675747,
+      "velocity" : 2.249445844270298,
+      "acceleration" : 3.999999999999989,
       "pose" : {
         "translation" : {
-          "x" : 6.947827837433621,
-          "y" : -2.8625370523589453
+          "x" : 6.9990385991161475,
+          "y" : -3.1359511128976223
         },
         "rotation" : {
-          "radians" : 2.684720287316673
+          "radians" : 2.903728300477056
         }
       },
-      "curvature" : 0.05916509233933032
+      "curvature" : -0.39618861692344265
     }, {
-      "time" : 0.6499414309623317,
-      "velocity" : 2.5997657238493272,
-      "acceleration" : 3.999999999999995,
+      "time" : 0.596630045456207,
+      "velocity" : 2.3865201818248267,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 6.868563832176733,
-          "y" : -2.8238541886530584
+          "x" : 6.922133978689089,
+          "y" : -3.116065460884556
         },
         "rotation" : {
-          "radians" : 2.690658352810391
+          "radians" : 2.873957256024724
         }
       },
-      "curvature" : 0.07535949039930706
+      "curvature" : -0.35341215031234335
     }, {
-      "time" : 0.6838872251295486,
-      "velocity" : 2.7355489005181948,
-      "acceleration" : 2.377223272165517,
+      "time" : 0.6297939149996428,
+      "velocity" : 2.5191756599985697,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 6.786916240142844,
-          "y" : -2.7846876997401324
+          "x" : 6.84398818281511,
+          "y" : -3.0934735449835173
         },
         "rotation" : {
-          "radians" : 2.6982158314592457
+          "radians" : 2.846951576467614
         }
       },
-      "curvature" : 0.09152947173359463
+      "curvature" : -0.31088845012056077
     }, {
-      "time" : 0.7171905270255866,
-      "velocity" : 2.8147182848254104,
-      "acceleration" : -3.999999999999995,
+      "time" : 0.6619472398665499,
+      "velocity" : 2.6477889594661983,
+      "acceleration" : 4.000000000000005,
       "pose" : {
         "translation" : {
-          "x" : 6.703254526793444,
-          "y" : -2.7454145583719765
+          "x" : 6.764803962199949,
+          "y" : -3.068372222252947
         },
         "rotation" : {
-          "radians" : 2.7074416804830337
+          "radians" : 2.822842297620444
         }
       },
-      "curvature" : 0.10819628898532277
+      "curvature" : -0.27006185244235026
     }, {
-      "time" : 0.7513282074668429,
-      "velocity" : 2.678167563060385,
-      "acceleration" : -4.0,
+      "time" : 0.6931403571893224,
+      "velocity" : 2.7725614287572884,
+      "acceleration" : -1.9238540319304083,
+      "pose" : {
+        "translation" : {
+          "x" : 6.684816072356853,
+          "y" : -3.041008955867703
+        },
+        "rotation" : {
+          "radians" : 2.8016585928334106
+        }
+      },
+      "curvature" : -0.23161048905860385
+    }, {
+      "time" : 0.724392316024546,
+      "velocity" : 2.7124372217464203,
+      "acceleration" : -3.9999999999999947,
       "pose" : {
         "translation" : {
-          "x" : 6.617985867988281,
-          "y" : -2.706431656110553
+          "x" : 6.604284376298892,
+          "y" : -3.011673162177658
         },
         "rotation" : {
-          "radians" : 2.7184067558532385
+          "radians" : 2.7833674081938065
         }
       },
-      "curvature" : 0.12588795042125855
+      "curvature" : -0.1956924881544235
     }, {
-      "time" : 0.7876117423160344,
-      "velocity" : 2.5330334236636194,
+      "time" : 0.7570834198223945,
+      "velocity" : 2.5816728065550265,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 6.531544461846352,
-          "y" : -2.6681462824344635
+          "x" : 6.523486947231277,
+          "y" : -2.980687557766295
         },
         "rotation" : {
-          "radians" : 2.7312047381163698
+          "radians" : 2.7679026528198674
         }
       },
-      "curvature" : 0.14513907009747126
+      "curvature" : -0.1621334305896103
     }, {
-      "time" : 0.8261977724199053,
-      "velocity" : 2.3786893032481355,
+      "time" : 0.7917065419550845,
+      "velocity" : 2.4431803180242664,
       "acceleration" : -3.999999999999995,
       "pose" : {
         "translation" : {
-          "x" : 6.4443808406068825,
-          "y" : -2.6309666038454367
+          "x" : 6.442713171243668,
+          "y" : -2.948399506509304
         },
         "rotation" : {
-          "radians" : 2.7459513677847163
+          "radians" : 2.7551856068980616
         }
       },
-      "curvature" : 0.16649109492446312
+      "curvature" : -0.13055898031005425
     }, {
-      "time" : 0.867316787787211,
-      "velocity" : 2.214213241778913,
-      "acceleration" : -3.999999999999995,
+      "time" : 0.8284398713448611,
+      "velocity" : 2.29624700046516,
+      "acceleration" : -3.9999999999999947,
       "pose" : {
         "translation" : {
-          "x" : 6.35695118249032,
-          "y" : -2.595292142974813
+          "x" : 6.362256850002495,
+          "y" : -2.915172366633179
         },
         "rotation" : {
-          "radians" : 2.7627816958449616
+          "radians" : 2.745138312714669
         }
       },
-      "curvature" : 0.1904846989403303
+      "curvature" : -0.10048447677557701
     }, {
-      "time" : 0.911319406879298,
-      "velocity" : 2.0382027654105648,
-      "acceleration" : -3.999999999999993,
+      "time" : 0.867530257770865,
+      "velocity" : 2.1398854547611443,
+      "acceleration" : -4.000000000000008,
       "pose" : {
         "translation" : {
-          "x" : 6.2697066235593155,
-          "y" : -2.561504257690032
+          "x" : 6.2824093034432735,
+          "y" : -2.8813768377738143
         },
         "rotation" : {
-          "radians" : 2.781844534128218
+          "radians" : 2.7376914272087047
         }
       },
-      "curvature" : 0.21763495429623342
+      "curvature" : -0.07137524263218635
     }, {
-      "time" : 0.9587586936046016,
-      "velocity" : 1.8484456185093505,
-      "acceleration" : -4.000000000000003,
+      "time" : 0.9093359695581078,
+      "velocity" : 1.9726626076121727,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 6.18308256957971,
-          "y" : -2.529956620201119
+          "x" : 6.203452472462914,
+          "y" : -2.847382308035101
         },
         "rotation" : {
-          "radians" : 2.803292700021926
+          "radians" : 2.7327875500302903
         }
       },
-      "curvature" : 0.24837648355682493
+      "curvature" : -0.04269098250129263
     }, {
-      "time" : 1.0105513298264461,
-      "velocity" : 1.6412750736219726,
-      "acceleration" : -4.000000000000001,
+      "time" : 0.9544024614642719,
+      "velocity" : 1.7923966399875164,
+      "acceleration" : -4.000000000000003,
       "pose" : {
         "translation" : {
-          "x" : 6.097488007881523,
-          "y" : -2.500965696167171
+          "x" : 6.125652021612041,
+          "y" : -2.8135482010475243
         },
         "rotation" : {
-          "radians" : 2.8272669567943085
+          "radians" : 2.7303805081764745
         }
       },
-      "curvature" : 0.28296053676338484
+      "curvature" : -0.013927070967790611
     }, {
-      "time" : 1.068337863711064,
-      "velocity" : 1.4101289380835003,
-      "acceleration" : -4.0,
+      "time" : 1.0036098078372346,
+      "velocity" : 1.5955672544956654,
+      "acceleration" : -3.9999999999999973,
       "pose" : {
         "translation" : {
-          "x" : 6.0132948192199365,
-          "y" : -2.4748012238028423
+          "x" : 6.049250441787308,
+          "y" : -2.7802153230267592
         },
         "rotation" : {
-          "radians" : 2.853870782635059
+          "radians" : 2.730430520921143
         }
       },
-      "curvature" : 0.3212799426490462
+      "curvature" : 0.015334192163236617
     }, {
-      "time" : 1.135467378593098,
-      "velocity" : 1.1416108785553647,
-      "acceleration" : -4.000000000000002,
+      "time" : 1.058499003222123,
+      "velocity" : 1.3760104729561113,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 5.930827089636281,
-          "y" : -2.451676692984833
+          "x" : 5.974460152923712,
+          "y" : -2.7476972098322676
         },
         "rotation" : {
-          "radians" : 2.883132411537973
+          "radians" : 2.732894603484722
         }
       },
-      "curvature" : 0.36259430688610855
+      "curvature" : 0.04534789601466625
     }, {
-      "time" : 1.2208700982319391,
-      "velocity" : 0.8,
-      "acceleration" : 0.0,
+      "time" : 1.1221487632123464,
+      "velocity" : 1.1214114329952183,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 5.850350422319025,
-          "y" : -2.431739824358374
+          "x" : 5.9014566066869065,
+          "y" : -2.716271474025895
         },
         "rotation" : {
-          "radians" : 2.9149503561792365
+          "radians" : 2.7377110228114296
         }
       },
-      "curvature" : 0.40513374346503805
+      "curvature" : 0.07609008232293428
     }, {
-      "time" : 1.3209271869450325,
+      "time" : 1.2025016214611508,
       "velocity" : 0.8,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.772061249464755,
-          "y" : -2.415063048443713
+          "x" : 5.830371389165521,
+          "y" : -2.6861711519304663
         },
         "rotation" : {
-          "radians" : 2.949019566392045
+          "radians" : 2.7447761809366122
         }
       },
-      "curvature" : 0.445587529202387
+      "curvature" : 0.10708399032696965
     }, {
-      "time" : 1.41738051101318,
+      "time" : 1.2959642165478331,
       "velocity" : 0.8,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.69607614413917,
-          "y" : -2.4016339847426025
+          "x" : 5.76128532356347,
+          "y" : -2.657576050688384
         },
         "rotation" : {
-          "radians" : 2.9847385905123645
+          "radians" : 2.7539121802444275
         }
       },
-      "curvature" : 0.47855128506289096
+      "curvature" : 0.13715838990065068
     }, {
-      "time" : 1.5103430795190167,
+      "time" : 1.3863197039855641,
       "velocity" : 0.8,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.622421132138058,
-          "y" : -2.3913459208447847
+          "x" : 5.694221572892275,
+          "y" : -2.6306040953202228
         },
         "rotation" : {
-          "radians" : 3.0211047999576164
+          "radians" : 2.764823929123116
         }
       },
-      "curvature" : 0.49612340761474616
+      "curvature" : 0.1641479856882942
     }, {
-      "time" : 1.600065855933919,
+      "time" : 1.473604587960989,
       "velocity" : 0.8,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.551021003848291,
-          "y" : -2.3839882915344788
+          "x" : 5.629138742663372,
+          "y" : -2.605302675783328
         },
         "rotation" : {
-          "radians" : 3.056615414973954
+          "radians" : 2.777046635977622
         }
       },
-      "curvature" : 0.4879890450440943
+      "curvature" : 0.1845929194913137
     }, {
-      "time" : 1.6869345775942237,
+      "time" : 1.5579775349142877,
       "velocity" : 0.8,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.481688626108806,
-          "y" : -2.379237157896867
+          "x" : 5.565923983580433,
+          "y" : -2.581639994030411
         },
         "rotation" : {
-          "radians" : 3.089205177495497
+          "radians" : 2.789888744275278
         }
       },
-      "curvature" : 0.44243031493165497
+      "curvature" : 0.1935775556708476
     }, {
-      "time" : 1.7714646340147757,
+      "time" : 1.6397283958798081,
       "velocity" : 0.8,
-      "acceleration" : -0.8733344992185003,
+      "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.414114254071592,
-          "y" : -2.376645686424581
+          "x" : 5.504386094231677,
+          "y" : -2.559496411068146
         },
         "rotation" : {
-          "radians" : 3.1162624888942854
+          "radians" : 2.8023823664689296
         }
       },
-      "curvature" : 0.3486274106696605
+      "curvature" : 0.18495835581671727
     }, {
-      "time" : 1.85842631949868,
-      "velocity" : 0.7240533599567176,
-      "acceleration" : -3.999999999999999,
+      "time" : 1.7192862234251227,
+      "velocity" : 0.8,
+      "acceleration" : -2.5807096371230194,
       "pose" : {
         "translation" : {
-          "x" : 5.347854843062677,
-          "y" : -2.375634628124189
+          "x" : 5.444248623782187,
+          "y" : -2.5386557940157672
         },
         "rotation" : {
-          "radians" : 3.1347683732234373
+          "radians" : 2.8132624602013823
         }
       },
-      "curvature" : 0.20027678859820805
+      "curvature" : 0.1523080406534184
     }, {
-      "time" : 2.0394396594878597,
+      "time" : 2.029278478459223,
       "velocity" : 0.0,
-      "acceleration" : -3.999999999999999,
+      "acceleration" : -2.5807096371230194,
       "pose" : {
         "translation" : {
-          "x" : 5.282323360443115,
-          "y" : -2.3754827976226807
+          "x" : 5.326601505279541,
+          "y" : -2.4994845390319824
         },
         "rotation" : {
-          "radians" : 3.141592653589793
+          "radians" : 2.823947779781842
         }
       },
       "curvature" : -0.0
     } ],
     "pointList" : [ {
-      "x" : [ 7.626592636108398, -1.5994892120361328, 0.0 ],
-      "y" : [ -3.196845054626465, 0.7553129196166992, 0.0 ]
+      "x" : [ 7.626592636108398, -2.086981773376465, 0.0 ],
+      "y" : [ -3.196845054626465, -0.026646852493286133, 0.0 ]
     }, {
-      "x" : [ 5.282323360443115, -2.0928425788879395, 0.0 ],
-      "y" : [ -2.3754827976226807, 0.0, 0.0 ]
+      "x" : [ 5.326601505279541, -1.8701248168945312, 0.0 ],
+      "y" : [ -2.4994845390319824, 0.6148552894592285, 0.0 ]
     } ],
     "rotations" : [ {
       "time" : 0.0,
       "rotation" : {
-        "radians" : -3.1179661408942683
+        "radians" : 2.9608083881863267
       }
     }, {
-      "time" : 1.9499999564141035,
+      "time" : 1.9199999570846558,
       "rotation" : {
         "radians" : -3.1066860685499065
       }
@@ -722,7 +736,7 @@
     } ]
   }, {
     "type" : "script",
-    "script" : "VisionManager.shootBalls 7",
+    "script" : "VisionManager.shootBalls 4.5",
     "closed" : false,
     "valid" : true,
     "sendableScript" : {
@@ -730,7 +744,7 @@
       "delay" : 0.0,
       "commands" : [ {
         "methodName" : "frc.subsystem.VisionManager.shootBalls",
-        "args" : [ "7" ],
+        "args" : [ "4.5" ],
         "argTypes" : [ "double" ],
         "reflection" : true
       } ]
@@ -760,809 +774,725 @@
     "states" : [ {
       "time" : 0.0,
       "velocity" : 0.0,
-      "acceleration" : 3.9999999999999996,
-      "pose" : {
-        "translation" : {
-          "x" : 5.282323360443115,
-          "y" : -2.3754827976226807
-        },
-        "rotation" : {
-          "radians" : -3.134549682568558
-        }
-      },
-      "curvature" : 0.0
-    }, {
-      "time" : 0.2485183417601868,
-      "velocity" : 0.9940733670407471,
-      "acceleration" : 4.000000000000001,
-      "pose" : {
-        "translation" : {
-          "x" : 5.158801735733213,
-          "y" : -2.376005907798927
-        },
-        "rotation" : {
-          "radians" : 3.1403960142550678
-        }
-      },
-      "curvature" : -0.1272575305163056
-    }, {
-      "time" : 0.35252878704229224,
-      "velocity" : 1.410115148169169,
-      "acceleration" : 4.000000000000002,
-      "pose" : {
-        "translation" : {
-          "x" : 5.033779233153609,
-          "y" : -2.3746042938932987
-        },
-        "rotation" : {
-          "radians" : 3.1185310345528885
-        }
-      },
-      "curvature" : -0.2154550840577468
-    }, {
-      "time" : 0.3949941940566982,
-      "velocity" : 1.579976776226793,
-      "acceleration" : 3.999999999999993,
-      "pose" : {
-        "translation" : {
-          "x" : 4.970320515414287,
-          "y" : -2.372685003625565
-        },
-        "rotation" : {
-          "radians" : 3.1038755506483433
-        }
-      },
-      "curvature" : -0.24455484294369947
-    }, {
-      "time" : 0.43379911025043366,
-      "velocity" : 1.7351964410017344,
-      "acceleration" : 3.9999999999999933,
-      "pose" : {
-        "translation" : {
-          "x" : 4.906065515041668,
-          "y" : -2.3697387947678834
-        },
-        "rotation" : {
-          "radians" : 3.0874576477610023
-        }
-      },
-      "curvature" : -0.2644121359909958
-    }, {
-      "time" : 0.4699166189846961,
-      "velocity" : 1.879666475938784,
-      "acceleration" : 3.9999999999999964,
-      "pose" : {
-        "translation" : {
-          "x" : 4.840914741610732,
-          "y" : -2.3656345444326052
-        },
-        "rotation" : {
-          "radians" : 3.069776012638989
-        }
-      },
-      "curvature" : -0.27597551307938123
-    }, {
-      "time" : 0.5039785450007895,
-      "velocity" : 2.0159141800031577,
       "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 4.77478698669438,
-          "y" : -2.3602638760203263
+          "x" : 5.326601505279541,
+          "y" : -2.4994845390319824
         },
         "rotation" : {
-          "radians" : 3.051283717312011
+          "radians" : 2.9473819457269834
         }
       },
-      "curvature" : -0.28036017762626425
+      "curvature" : -0.0
     }, {
-      "time" : 0.5364200087481645,
-      "velocity" : 2.1456800349926577,
-      "acceleration" : 3.999999999999987,
+      "time" : 0.22616825941529703,
+      "velocity" : 0.9046730376611881,
+      "acceleration" : 3.9999999999999982,
       "pose" : {
         "translation" : {
-          "x" : 4.707618566644907,
-          "y" : -2.3535402315285237
+          "x" : 5.226273182620389,
+          "y" : -2.479475304490734
         },
         "rotation" : {
-          "radians" : 3.0323821587711612
+          "radians" : 2.940693415172918
         }
       },
-      "curvature" : -0.2787476309858388
+      "curvature" : -0.08516764713670827
     }, {
-      "time" : 0.5675529868653102,
-      "velocity" : 2.27021194746124,
-      "acceleration" : 3.999999999999994,
+      "time" : 0.28478553771106996,
+      "velocity" : 1.1391421508442798,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 4.639362565375471,
-          "y" : -2.345397943860192
+          "x" : 5.167604320016238,
+          "y" : -2.467386164908845
         },
         "rotation" : {
-          "radians" : 3.0134184909093236
+          "radians" : 2.9363469447848343
         }
       },
-      "curvature" : -0.2723037406158022
+      "curvature" : -0.057690329781864697
     }, {
-      "time" : 0.5976074193052028,
-      "velocity" : 2.3904296772208102,
-      "acceleration" : 3.9999999999999813,
+      "time" : 0.34012838221283126,
+      "velocity" : 1.360513528851325,
+      "acceleration" : 3.9999999999999987,
       "pose" : {
         "translation" : {
-          "x" : 4.569988077141569,
-          "y" : -2.3357913091324796
+          "x" : 5.099909630283946,
+          "y" : -2.453181116929045
         },
         "rotation" : {
-          "radians" : 2.994685983003478
+          "radians" : 2.9335394561142465
         }
       },
-      "curvature" : -0.2621176699160821
+      "curvature" : -0.024959128681885406
     }, {
-      "time" : 0.6267558907189836,
-      "velocity" : 2.507023562875933,
-      "acceleration" : 4.000000000000006,
+      "time" : 0.39469640970120917,
+      "velocity" : 1.5787856388048367,
+      "acceleration" : 3.999999999999998,
       "pose" : {
         "translation" : {
-          "x" : 4.499479449322507,
-          "y" : -2.324693658985325
+          "x" : 5.021453386801028,
+          "y" : -2.4365679640294218
         },
         "rotation" : {
-          "radians" : 2.9764266585803343
+          "radians" : 2.9326521624329898
         }
       },
-      "curvature" : -0.24916158984645412
+      "curvature" : 7.654668522091257E-4
     }, {
-      "time" : 0.6551293302859829,
-      "velocity" : 2.62051732114393,
-      "acceleration" : 4.000000000000006,
+      "time" : 0.4494207725602909,
+      "velocity" : 1.7976830902411636,
+      "acceleration" : 4.000000000000004,
       "pose" : {
         "translation" : {
-          "x" : 4.427835525202872,
-          "y" : -2.3120964328900926
+          "x" : 4.931068547291034,
+          "y" : -2.4174357876372596
         },
         "rotation" : {
-          "radians" : 2.9588355536058346
+          "radians" : 2.933612583177434
         }
       },
-      "curvature" : -0.23426901034532097
+      "curvature" : 0.018438020867265596
     }, {
-      "time" : 0.6828274897926779,
-      "velocity" : 2.7313099591707104,
-      "acceleration" : 4.0,
+      "time" : 0.5045581168818597,
+      "velocity" : 2.0182324675274392,
+      "acceleration" : 4.0000000000000036,
       "pose" : {
         "translation" : {
-          "x" : 4.355068886754005,
-          "y" : -2.29800825045821
+          "x" : 4.8281097953876895,
+          "y" : -2.3958376963394414
         },
         "rotation" : {
-          "radians" : 2.942065978126006
+          "radians" : 2.9362264556688444
         }
       },
-      "curvature" : -0.21812823894940492
+      "curvature" : 0.030240172077865078
     }, {
-      "time" : 0.7099262281663108,
-      "velocity" : 2.839704912665242,
-      "acceleration" : 2.249078927171693,
+      "time" : 0.5600430300070354,
+      "velocity" : 2.240172120028142,
+      "acceleration" : 3.999999999999993,
       "pose" : {
         "translation" : {
-          "x" : 4.281205097415471,
-          "y" : -2.2824539837498032
+          "x" : 4.712406582199037,
+          "y" : -2.371973575092852
         },
         "rotation" : {
-          "radians" : 2.9262352542189807
+          "radians" : 2.940317933492544
         }
       },
-      "curvature" : -0.20128694960793697
+      "curvature" : 0.038432989629750024
     }, {
-      "time" : 0.7366956832801526,
-      "velocity" : 2.899911530053652,
-      "acceleration" : 0.6075869985665846,
+      "time" : 0.587849840307584,
+      "velocity" : 2.3513993612303365,
+      "acceleration" : 3.9999999999999867,
       "pose" : {
         "translation" : {
-          "x" : 4.206281944876537,
-          "y" : -2.2654738295823336
+          "x" : 4.649840769147667,
+          "y" : -2.359290106139965
         },
         "rotation" : {
-          "radians" : 2.9114305148203647
+          "radians" : 2.9428792118280978
         }
       },
-      "curvature" : -0.18416398315815855
+      "curvature" : 0.041716157528885855
     }, {
-      "time" : 0.7635586220886558,
-      "velocity" : 2.9162331024169883,
-      "acceleration" : 0.6121916771139427,
+      "time" : 0.6156531468331837,
+      "velocity" : 2.462612587332735,
+      "acceleration" : 3.999999999999987,
       "pose" : {
         "translation" : {
-          "x" : 4.130348683857635,
-          "y" : -2.2471223818392336
+          "x" : 4.584216167871574,
+          "y" : -2.346172834434782
         },
         "rotation" : {
-          "radians" : 2.8977142638407907
+          "radians" : 2.9457720291811604
         }
       },
-      "curvature" : -0.1670650670986652
+      "curvature" : 0.04467007920827488
     }, {
-      "time" : 0.790693133931038,
-      "velocity" : 2.9328446247294444,
-      "acceleration" : 0.610901889033365,
+      "time" : 0.643414376324417,
+      "velocity" : 2.5736575052976676,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 4.053465278891842,
-          "y" : -2.2274677037785438
+          "x" : 4.515624076932947,
+          "y" : -2.3326803444587343
         },
         "rotation" : {
-          "radians" : 2.885129504199819
+          "radians" : 2.9489923575381187
         }
       },
-      "curvature" : -0.1501999014049682
+      "curvature" : 0.04741776873328391
     }, {
-      "time" : 0.8180687543276729,
-      "velocity" : 2.949568442943209,
-      "acceleration" : 0.6061597220097747,
+      "time" : 0.671093384815894,
+      "velocity" : 2.6843735392635755,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 3.975701647106349,
-          "y" : -2.206590400341548
+          "x" : 4.444176663154394,
+          "y" : -2.3188771596933293
         },
         "rotation" : {
-          "radians" : 2.873704328992082
+          "radians" : 2.9525400109302637
         }
       },
-      "curvature" : -0.13369882843621037
+      "curvature" : 0.05006038596145867
     }, {
-      "time" : 0.8456518976913671,
-      "velocity" : 2.9662882334567016,
-      "acceleration" : 0.6000067617510872,
+      "time" : 0.6986493044854305,
+      "velocity" : 2.7945972179417216,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 3.8971369010039325,
-          "y" : -2.1845826904614114
+          "x" : 4.370005494167825,
+          "y" : -2.304833203532975
         },
         "rotation" : {
-          "radians" : 2.8634559390901275
+          "radians" : 2.956418464768416
         }
       },
-      "curvature" : -0.11762797784051136
+      "curvature" : 0.05268264317257482
     }, {
-      "time" : 0.8734057728964819,
-      "velocity" : 2.9829407462445663,
-      "acceleration" : 0.5941482062082035,
+      "time" : 0.7260411741134952,
+      "velocity" : 2.9041646964539805,
+      "acceleration" : 3.5173178369045623,
       "pose" : {
         "translation" : {
-          "x" : 3.817858591244428,
-          "y" : -2.1615474793718144
+          "x" : 4.293260070963328,
+          "y" : -2.2906232601978047
         },
         "rotation" : {
-          "radians" : 2.854394098775556
+          "radians" : 2.960634714311987
         }
       },
-      "curvature" : -0.10200230298518619
+      "curvature" : 0.055357358915842876
     }, {
-      "time" : 0.9012903734356116,
-      "velocity" : 2.9995083316357225,
-      "acceleration" : 0.017530271310819877,
+      "time" : 0.7532878740109261,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 3.7379619494262,
-          "y" : -2.1375974309155907
+          "x" : 4.214106360438051,
+          "y" : -2.276326435646504
         },
         "rotation" : {
-          "radians" : 2.8465240722371243
+          "radians" : 2.965199184647632
         }
       },
-      "curvature" : -0.08679629840563431
+      "curvature" : 0.05814923970169821
     }, {
-      "time" : 0.9293371934510891,
-      "velocity" : 2.9999999999999996,
-      "acceleration" : 1.0483666419050847E-14,
+      "time" : 0.7808305395419614,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 3.6575491308676185,
-          "y" : -2.1128540398533633
+          "x" : 4.1327253279450815,
+          "y" : -2.262025618489133
         },
         "rotation" : {
-          "radians" : 2.8398491015703136
+          "radians" : 2.970125697805022
         }
       },
-      "curvature" : -0.07195242715558157
+      "curvature" : 0.061118028567780355
     }, {
-      "time" : 0.9575772634227335,
+      "time" : 0.8090362176857757,
       "velocity" : 3.0,
-      "acceleration" : -1.0429522860436319E-14,
+      "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 3.5767284573885263,
-          "y" : -2.08744670417218
+          "x" : 4.049311469842324,
+          "y" : -2.247806940899955
         },
         "rotation" : {
-          "radians" : 2.834372494847544
+          "radians" : 2.975431499212657
         }
       },
-      "curvature" : -0.05738742170266805
+      "curvature" : 0.06432117010498879
     }, {
-      "time" : 0.985963938172396,
-      "velocity" : 2.9999999999999996,
-      "acceleration" : 1.039470354847096E-14,
+      "time" : 0.8378328544866487,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 3.4956136600917134,
-          "y" : -2.0615117973941506
+          "x" : 3.9640713460413792,
+          "y" : -2.233759239530258
         },
         "rotation" : {
-          "radians" : 2.8300993940344488
+          "radians" : 2.981137344223006
         }
       },
-      "curvature" : -0.0429966817331539
+      "curvature" : 0.06781613205292471
     }, {
-      "time" : 1.014445700240245,
+      "time" : 0.8671450353421751,
       "velocity" : 3.0,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 3.4143231221443884,
-          "y" : -2.0351917408850824
+          "x" : 3.877222112556427,
+          "y" : -2.219973516421182
         },
         "rotation" : {
-          "radians" : 2.8270382894922435
+          "radians" : 2.987267645068647
         }
       },
-      "curvature" : -0.028657002421013374
+      "curvature" : 0.07166250783511754
     }, {
-      "time" : 1.0429688993490678,
+      "time" : 0.8968945284065684,
       "velocity" : 3.0,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 3.332979121559652,
-          "y" : -2.008634076163117
+          "x" : 3.7889900540531016,
+          "y" : -2.206542399916543
         },
         "rotation" : {
-          "radians" : 2.8252023423513197
+          "radians" : 2.9938506788290518
         }
       },
-      "curvature" : -0.014227847229079386
+      "curvature" : 0.07592400875189884
     }, {
-      "time" : 1.0714782036513595,
+      "time" : 0.92700083074625,
       "velocity" : 3.0,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 3.2517070739779683,
-          "y" : -1.9819905372073663
+          "x" : 3.699609116397373,
+          "y" : -2.1935596055756603
         },
         "rotation" : {
-          "radians" : 2.824610569391105
+          "radians" : 3.000918857463063
         }
       },
-      "curvature" : 4.486554553824595E-4
+      "curvature" : 0.08067044151870519
     }, {
-      "time" : 1.0999170602694552,
+      "time" : 0.9573817176378253,
       "velocity" : 3.0,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 3.1706347754486357,
-          "y" : -1.955416122766549
+          "x" : 3.6093194392044268,
+          "y" : -2.181119397086178
         },
         "rotation" : {
-          "radians" : 2.82528893793973
+          "radians" : 3.008509061471997
         }
       },
-      "curvature" : 0.015548852554256204
+      "curvature" : 0.08597975704502848
     }, {
-      "time" : 1.1282281648158434,
+      "time" : 0.9879537954490873,
       "velocity" : 3.0,
-      "acceleration" : 0.0,
+      "acceleration" : -0.06708023937809733,
       "pose" : {
         "translation" : {
-          "x" : 3.08989164521126,
-          "y" : -1.9290681686676265
+          "x" : 3.5183658883875415,
+          "y" : -2.169316047176893
         },
         "rotation" : {
-          "radians" : 2.827271411029802
+          "radians" : 3.0166630391262212
         }
       },
-      "curvature" : 0.03126974228812179
+      "curvature" : 0.0919402494188015
     }, {
-      "time" : 1.1563539399890645,
-      "velocity" : 3.0,
-      "acceleration" : 0.0,
+      "time" : 1.0186435886693106,
+      "velocity" : 2.997941321324323,
+      "acceleration" : -0.22390997531894352,
       "pose" : {
         "translation" : {
-          "x" : 3.009607968477228,
-          "y" : -1.9031054201244397
+          "x" : 3.42699658870697,
+          "y" : -2.1582432985305786
         },
         "rotation" : {
-          "radians" : 2.8306009755902637
+          "radians" : 3.0254278732409468
         }
       },
-      "curvature" : 0.04783211564223483
+      "curvature" : 0.09865297901559254
     }, {
-      "time" : 1.184237023700504,
-      "velocity" : 3.0,
-      "acceleration" : 0.0,
+      "time" : 1.0494023957488516,
+      "velocity" : 2.991054117590303,
+      "acceleration" : -0.24968095645801092,
       "pose" : {
         "translation" : {
-          "x" : 2.929914139211178,
-          "y" : -1.8776871040463448
+          "x" : 3.3354614563188187,
+          "y" : -2.147993824696811
         },
         "rotation" : {
-          "radians" : 2.8353306784934555
+          "radians" : 3.034856517018437
         }
       },
-      "curvature" : 0.06548461110054977
+      "curvature" : 0.1062344889765406
     }, {
-      "time" : 1.2118207675049235,
-      "velocity" : 3.0,
-      "acceleration" : -0.7663356953505052,
+      "time" : 1.0801755500252042,
+      "velocity" : 2.9833706469973533,
+      "acceleration" : -0.2793532278338764,
       "pose" : {
         "translation" : {
-          "x" : 2.8509399029124705,
-          "y" : -1.8529720013468496
+          "x" : 3.2440107313239253,
+          "y" : -2.1386586910047924
         },
         "rotation" : {
-          "radians" : 2.8415246861848655
+          "radians" : 3.045008399208256
         }
       },
-      "curvature" : 0.08450862897758021
+      "curvature" : 0.11481987777652654
     }, {
-      "time" : 1.2391451065801053,
-      "velocity" : 2.9790603836148275,
-      "acceleration" : -0.900862157373095,
+      "time" : 1.1108888389022582,
+      "velocity" : 2.974790790612154,
+      "acceleration" : -0.31347889545111546,
       "pose" : {
         "translation" : {
-          "x" : 2.772813599396663,
-          "y" : -1.8291175192522502
+          "x" : 3.1528935103167397,
+          "y" : -2.1303268154761783
         },
         "rotation" : {
-          "radians" : 2.8492593724376376
+          "radians" : 3.055950096387905
         }
       },
-      "curvature" : 0.1052240209046203
+      "curvature" : 0.12456627894671345
     }, {
-      "time" : 1.2662653693057482,
-      "velocity" : 2.95462876522728,
-      "acceleration" : -1.4327348136338478,
+      "time" : 1.1414701580581714,
+      "velocity" : 2.9652041924617203,
+      "acceleration" : -0.3526686678603589,
       "pose" : {
         "translation" : {
-          "x" : 2.695661405576982,
-          "y" : -1.8062787636102668
+          "x" : 3.062356278934203,
+          "y" : -2.123084429737901
         },
         "rotation" : {
-          "radians" : 2.8586244241224126
+          "radians" : 3.0677560659844088
         }
       },
-      "curvature" : 0.12799539517209996
+      "curvature" : 0.13565677726404682
     }, {
-      "time" : 1.2932068425100405,
-      "velocity" : 2.916028778636907,
-      "acceleration" : -3.9999999999999942,
+      "time" : 1.1718500837875183,
+      "velocity" : 2.954490144525055,
+      "acceleration" : -0.3975769717522214,
       "pose" : {
         "translation" : {
-          "x" : 2.6196065782457936,
-          "y" : -1.7846076111986804
+          "x" : 2.972641444404627,
+          "y" : -2.1170145399349947
         },
         "rotation" : {
-          "radians" : 2.8697239348789982
+          "radians" : 3.080509426966478
         }
       },
-      "curvature" : 0.15323873749770084
+      "curvature" : 0.14830475135070306
     }, {
-      "time" : 1.320307309562304,
-      "velocity" : 2.8076269104278535,
-      "acceleration" : -4.000000000000006,
+      "time" : 1.2019624393536652,
+      "velocity" : 2.94251816538674,
+      "acceleration" : -0.44887369505519986,
       "pose" : {
         "translation" : {
-          "x" : 2.5447686968560754,
-          "y" : -1.7642517820339685
+          "x" : 2.8839858680965733,
+          "y" : -2.112196387643422
         },
         "rotation" : {
-          "radians" : 2.882677428692841
+          "radians" : 3.0943027648494486
         }
       },
-      "curvature" : 0.18142780690271726
+      "curvature" : 0.16275856428249422
     }, {
-      "time" : 1.3478810613702046,
-      "velocity" : 2.6973319031962504,
-      "acceleration" : -3.9999999999999942,
+      "time" : 1.231744849146722,
+      "velocity" : 2.9291496250552824,
+      "acceleration" : -0.5071956035960418,
       "pose" : {
         "translation" : {
-          "x" : 2.47126290630289,
-          "y" : -1.745353911679942
+          "x" : 2.7966193980677323,
+          "y" : -2.1087049107828983
         },
         "rotation" : {
-          "radians" : 2.8976207163334644
+          "radians" : 3.109238922273074
         }
       },
-      "curvature" : 0.21309938210883872
+      "curvature" : 0.1793064095821939
     }, {
-      "time" : 1.3759409012078119,
-      "velocity" : 2.5850925438458217,
-      "acceleration" : -4.000000000000006,
+      "time" : 1.2611392719425794,
+      "velocity" : 2.9142409030429803,
+      "acceleration" : -0.9399899449235506,
       "pose" : {
         "translation" : {
-          "x" : 2.3991991597048568,
-          "y" : -1.7280506235563808
+          "x" : 2.7107634016138036,
+          "y" : -2.1066102045297157
         },
         "rotation" : {
-          "radians" : 2.9147064333256036
+          "radians" : 3.12543171391447
         }
       },
-      "curvature" : 0.2488558447958357
+      "curvature" : 0.19828093525498974
     }, {
-      "time" : 1.4045086672921978,
-      "velocity" : 2.470821479508278,
+      "time" : 1.290145772258879,
+      "velocity" : 2.886975084408237,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.3286814611856244,
-          "y" : -1.712471601247671
+          "x" : 2.6266292978173746,
+          "y" : -2.1059769822295697
         },
         "rotation" : {
-          "radians" : 2.934104033894702
+          "radians" : -3.140178834940239
         }
       },
-      "curvature" : 0.2893627194755785
+      "curvature" : 0.2200629785863679
     }, {
-      "time" : 1.4336183671333464,
-      "velocity" : 2.354382680143684,
-      "acceleration" : -4.0,
+      "time" : 1.3192095490289386,
+      "velocity" : 2.7707199773279982,
+      "acceleration" : -4.000000000000005,
       "pose" : {
         "translation" : {
-          "x" : 2.259807108655341,
-          "y" : -1.6987386608114403
+          "x" : 2.5444170900968004,
+          "y" : -2.106864036310384
         },
         "rotation" : {
-          "radians" : 2.95599891646069
+          "radians" : -3.121085021922299
         }
       },
-      "curvature" : 0.3353375731902151
+      "curvature" : 0.24508330253873933
     }, {
-      "time" : 1.463320468012547,
-      "velocity" : 2.2355742766268825,
-      "acceleration" : -4.0,
+      "time" : 1.3487642900235868,
+      "velocity" : 2.6525010133494056,
+      "acceleration" : -4.000000000000005,
       "pose" : {
         "translation" : {
-          "x" : 2.192665936592128,
-          "y" : -1.6869648230871945
+          "x" : 2.4643138987550826,
+          "y" : -2.1093236991951345
         },
         "rotation" : {
-          "radians" : 2.9805902295653475
+          "radians" : -3.1003235820288793
         }
       },
-      "curvature" : 0.38752507295857674
+      "curvature" : 0.27382057237627916
     }, {
-      "time" : 1.4936878988257565,
-      "velocity" : 2.1141045533740446,
-      "acceleration" : -3.9999999999999964,
+      "time" : 1.378824758299442,
+      "velocity" : 2.532259140245985,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.127339558823553,
-          "y" : -1.677253386004954
+          "x" : 2.386492493528749,
+          "y" : -2.113401304214676
         },
         "rotation" : {
-          "radians" : 3.0080867539704594
+          "radians" : -3.0777355237933675
         }
       },
-      "curvature" : 0.4466510713484467
+      "curvature" : 0.3067928849363109
     }, {
-      "time" : 1.524824670531073,
-      "velocity" : 1.9895574665527784,
-      "acceleration" : -3.830124802213549,
+      "time" : 1.409418937364567,
+      "velocity" : 2.409882423985485,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.0639006113080995,
-          "y" : -1.6696969968938902
+          "x" : 2.3111098261367333,
+          "y" : -2.1191346465205667
         },
         "rotation" : {
-          "radians" : 3.0387000883556468
+          "radians" : -3.053153686544722
         }
       },
-      "curvature" : 0.513346644844465
+      "curvature" : 0.3445389130934825
     }, {
-      "time" : 1.5898426254464024,
-      "velocity" : 1.7405305848423733,
-      "acceleration" : -2.9205367698139653,
+      "time" : 1.440592604317041,
+      "velocity" : 2.2851877561755893,
+      "acceleration" : -4.000000000000006,
       "pose" : {
         "translation" : {
-          "x" : 1.9429261182139115,
-          "y" : -1.6613611327495477
+          "x" : 2.238305562829254,
+          "y" : -2.126553443997892
         },
         "rotation" : {
-          "radians" : 3.110070421404804
+          "radians" : -3.0264066985780818
         }
       },
-      "curvature" : 0.6707495427301515
+      "curvature" : 0.38758316287414446
     }, {
-      "time" : 1.6586294726277089,
-      "velocity" : 1.539636068369794,
-      "acceleration" : -2.312476902063167,
+      "time" : 1.472415714693242,
+      "velocity" : 2.157895314670785,
+      "acceleration" : -3.812563066154557,
       "pose" : {
         "translation" : {
-          "x" : 1.8301152132917196,
-          "y" : -1.6624501449987292
+          "x" : 2.1682006169366934,
+          "y" : -2.135678798178092
         },
         "rotation" : {
-          "radians" : -3.0872540859948607
+          "radians" : -2.997325314341709
         }
       },
-      "curvature" : 0.857210634989987
+      "curvature" : 0.43637811252079767
     }, {
-      "time" : 1.694295911389866,
-      "velocity" : 1.4571582525534557,
-      "acceleration" : -1.935712207829296,
+      "time" : 1.504942660922755,
+      "velocity" : 2.0338842808213484,
+      "acceleration" : -3.4745285073926646,
       "pose" : {
         "translation" : {
-          "x" : 1.7768357257042826,
-          "y" : -1.6666209962559293
+          "x" : 2.1008956814184785,
+          "y" : -2.1465226551517844
         },
         "rotation" : {
-          "radians" : -3.038795429850023
+          "radians" : -2.965751945842899
         }
       },
-      "curvature" : 0.9569963097495596
+      "curvature" : 0.4912145518613118
     }, {
-      "time" : 1.7305902897747223,
-      "velocity" : 1.3869027812383132,
-      "acceleration" : -1.5697273044131261,
+      "time" : 1.5381580958310392,
+      "velocity" : 1.918476305347069,
+      "acceleration" : -3.1316450892116623,
       "pose" : {
         "translation" : {
-          "x" : 1.7256485446325698,
-          "y" : -1.6732271661251303
+          "x" : 2.0364697614119587,
+          "y" : -2.159087266481591
         },
         "rotation" : {
-          "radians" : -2.9868346157105776
+          "radians" : -2.9315543011342946
         }
       },
-      "curvature" : 1.0564079612638406
+      "curvature" : 0.5520911992533699
     }, {
-      "time" : 1.76735655145933,
-      "velocity" : 1.3291897763907865,
-      "acceleration" : -1.2076785428926229,
+      "time" : 1.571997357302157,
+      "velocity" : 1.8125037483384936,
+      "acceleration" : -2.6083220889136767,
       "pose" : {
         "translation" : {
-          "x" : 1.676542258832705,
-          "y" : -1.6822607723713734
+          "x" : 1.974978706781286,
+          "y" : -2.173364650114962
         },
         "rotation" : {
-          "radians" : -2.9317169913107692
+          "radians" : -2.8946440185387496
         }
       },
-      "curvature" : 1.1501373820361844
+      "curvature" : 0.6185372751982955
     }, {
-      "time" : 1.8044095556051742,
-      "velocity" : 1.284441658334139,
-      "acceleration" : -0.8415938937413784,
+      "time" : 1.6410415980225266,
+      "velocity" : 1.6324141301552804,
+      "acceleration" : -2.056186097930853,
       "pose" : {
         "translation" : {
-          "x" : 1.6294904214435064,
-          "y" : -1.6936958606279404
+          "x" : 1.8609000120313794,
+          "y" : -2.206971403483294
         },
         "rotation" : {
-          "radians" : -2.8739898269443005
+          "radians" : -2.8127029122723353
         }
       },
-      "curvature" : 1.231671630794503
+      "curvature" : 0.762540672124892
     }, {
-      "time" : 1.8415337315942304,
-      "velocity" : 1.253198178511569,
-      "acceleration" : -0.4610275989179017,
+      "time" : 1.6761338135036778,
+      "velocity" : 1.5602580045373433,
+      "acceleration" : -1.6675516226568237,
       "pose" : {
         "translation" : {
-          "x" : 1.5844507927679574,
-          "y" : -1.7074874767049923
+          "x" : 1.8082950882143756,
+          "y" : -2.2262287892527253
         },
         "rotation" : {
-          "radians" : -2.8144162659479433
+          "radians" : -2.7679602634610805
         }
       },
-      "curvature" : 1.2938507850253793
+      "curvature" : 0.8347008677720427
     }, {
-      "time" : 1.8784830733784548,
-      "velocity" : 1.236163512187191,
-      "acceleration" : -0.05084780300878941,
+      "time" : 1.7113375846273613,
+      "velocity" : 1.5015538988764054,
+      "acceleration" : -1.2481101573777,
       "pose" : {
         "translation" : {
-          "x" : 1.5413645830546798,
-          "y" : -1.7235707388982036
+          "x" : 1.7585875274754414,
+          "y" : -2.247053901220312
         },
         "rotation" : {
-          "radians" : -2.753967325858055
+          "radians" : -2.7211516082019864
         }
       },
-      "curvature" : 1.3297557068700718
+      "curvature" : 0.9012428901840408
     }, {
-      "time" : 1.914982265295949,
-      "velocity" : 1.2343076084665903,
-      "acceleration" : 0.4127671665855387,
+      "time" : 1.7464375053486416,
+      "velocity" : 1.4577453313010234,
+      "acceleration" : -0.7779237166793617,
       "pose" : {
         "translation" : {
-          "x" : 1.5001556952794068,
-          "y" : -1.7418599102973964
+          "x" : 1.7116953915459305,
+          "y" : -2.2693795029500246
         },
         "rotation" : {
-          "radians" : -2.6937880813077553
+          "radians" : -2.672856659027319
         }
       },
-      "curvature" : 1.333757551972331
+      "curvature" : 0.9562256414450673
     }, {
-      "time" : 1.9507281593593455,
-      "velocity" : 1.2490623398762053,
-      "acceleration" : 0.9702157730169337,
+      "time" : 1.781173038934524,
+      "velocity" : 1.430723735913053,
+      "acceleration" : -0.22157809100517403,
       "pose" : {
         "translation" : {
-          "x" : 1.4607299679264543,
-          "y" : -1.7622474710951792
+          "x" : 1.667504782177275,
+          "y" : -2.293124889867613
         },
         "rotation" : {
-          "radians" : -2.635140362252442
+          "radians" : -2.623879203384706
         }
       },
-      "curvature" : 1.3024332506879337
+      "curvature" : 0.9926865535315983
     }, {
-      "time" : 1.9853901192083565,
-      "velocity" : 1.2826919200453955,
-      "acceleration" : 1.6954572937803598,
+      "time" : 1.8152328314093547,
+      "velocity" : 1.4231768321164475,
+      "acceleration" : 0.4873333149229415,
       "pose" : {
         "translation" : {
-          "x" : 1.4229744177701917,
-          "y" : -1.7846031908955844
+          "x" : 1.625868373689869,
+          "y" : -2.318195350173432
         },
         "rotation" : {
-          "radians" : -2.5793313706904004
+          "radians" : -2.5752533397836075
         }
       },
-      "curvature" : 1.2350342042717206
+      "curvature" : 1.003242618645744
     }, {
-      "time" : 2.0186069121127237,
-      "velocity" : 1.3390095738510968,
-      "acceleration" : -2.34726952828823,
+      "time" : 1.8482473398234809,
+      "velocity" : 1.439265901942455,
+      "acceleration" : -3.6589651945051656,
       "pose" : {
         "translation" : {
-          "x" : 1.3867564826565157,
-          "y" : -1.808773201022703
+          "x" : 1.5866039455219436,
+          "y" : -2.3444816257552645
         },
         "rotation" : {
-          "radians" : -2.5276425781012595
+          "radians" : -2.5282276575480873
         }
       },
-      "curvature" : 1.1333298805180545
+      "curvature" : 0.9809381622274879
     }, {
-      "time" : 2.0874348859976477,
-      "velocity" : 1.177451768057196,
+      "time" : 1.9178314402693484,
+      "velocity" : 1.1846601003200747,
       "acceleration" : -4.000000000000001,
       "pose" : {
         "translation" : {
-          "x" : 1.3183007709869798,
-          "y" : -1.861816860005547
+          "x" : 1.5142788687799111,
+          "y" : -2.4001886242122055
         },
         "rotation" : {
-          "radians" : -2.441312318087089
+          "radians" : -2.44479080185293
         }
       },
-      "curvature" : 0.8419545117782058
+      "curvature" : 0.8173776396794599
     }, {
-      "time" : 2.1735484520729687,
-      "velocity" : 0.8329975037559129,
-      "acceleration" : -4.0,
+      "time" : 2.0054115856723604,
+      "velocity" : 0.834339518708027,
+      "acceleration" : -3.9999999999999996,
       "pose" : {
         "translation" : {
-          "x" : 1.2538819828114995,
-          "y" : -1.919639480765781
+          "x" : 1.4483181266711824,
+          "y" : -2.4590604087766508
         },
         "rotation" : {
-          "radians" : -2.384418923560335
+          "radians" : -2.3859695614811485
         }
       },
-      "curvature" : 0.4592519163026959
+      "curvature" : 0.48392472113324264
     }, {
-      "time" : 2.3817978280119467,
+      "time" : 2.2139964653493673,
       "velocity" : 0.0,
-      "acceleration" : -4.0,
+      "acceleration" : -3.9999999999999996,
       "pose" : {
         "translation" : {
-          "x" : 1.191657543182373,
-          "y" : -1.9800645112991333
+          "x" : 1.3858580589294434,
+          "y" : -2.519644260406494
         },
         "rotation" : {
           "radians" : -2.3639463990252185
@@ -1571,11 +1501,11 @@
       "curvature" : 0.0
     } ],
     "pointList" : [ {
-      "x" : [ 5.282323360443115, -3.944063901901245, 0.0 ],
-      "y" : [ -2.3754827976226807, -0.02777838706970215, 0.0 ]
+      "x" : [ 5.326601505279541, -1.5258679389953613, 0.0 ],
+      "y" : [ -2.4994845390319824, 0.3001227378845215, 0.0 ]
     }, {
-      "x" : [ 1.191657543182373, -1.9787755012512207, 0.0 ],
-      "y" : [ -1.9800645112991333, -1.9483323097229004, 0.0 ]
+      "x" : [ 1.3858580589294434, -1.9787755012512207, 0.0 ],
+      "y" : [ -2.519644260406494, -1.9483323097229004, 0.0 ]
     } ],
     "rotations" : [ {
       "time" : 0.0,
@@ -1583,7 +1513,7 @@
         "radians" : -2.3736477827122884
       }
     }, {
-      "time" : 2.2899999488145113,
+      "time" : 2.1199999526143074,
       "rotation" : {
         "radians" : -2.3824463294692526
       }
@@ -1639,11 +1569,11 @@
     "states" : [ {
       "time" : 0.0,
       "velocity" : 0.0,
-      "acceleration" : 4.0,
+      "acceleration" : 3.999999999999999,
       "pose" : {
         "translation" : {
-          "x" : 1.191657543182373,
-          "y" : -1.9800645112991333
+          "x" : 1.3858580589294434,
+          "y" : -2.519644260406494
         },
         "rotation" : {
           "radians" : 0.0
@@ -1651,223 +1581,349 @@
       },
       "curvature" : 0.0
     }, {
-      "time" : 0.24958615457676173,
-      "velocity" : 0.9983446183070469,
-      "acceleration" : 3.9999999999999973,
+      "time" : 0.2506613508834166,
+      "velocity" : 1.0026454035336663,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 1.3162440362680172,
-          "y" : -1.9800961888311122
+          "x" : 1.5115202584765939,
+          "y" : -2.5197252612570082
         },
         "rotation" : {
-          "radians" : -7.425481719022404E-4
+          "radians" : -0.0018522539500240682
         }
       },
-      "curvature" : -0.011304718810550897
+      "curvature" : -0.026842794063227322
     }, {
-      "time" : 0.3514312438034884,
-      "velocity" : 1.4057249752139533,
-      "acceleration" : 4.000000000000001,
+      "time" : 0.30789720119155506,
+      "velocity" : 1.23158880476622,
+      "acceleration" : 4.000000000000002,
       "pose" : {
         "translation" : {
-          "x" : 1.4386652179964585,
-          "y" : -1.9802937444110285
+          "x" : 1.5754591548339363,
+          "y" : -2.519904416002504
         },
         "rotation" : {
-          "radians" : -0.0026512076017559455
+          "radians" : -0.003832966982418239
         }
       },
-      "curvature" : -0.019459649604384354
+      "curvature" : -0.03448311037462077
     }, {
-      "time" : 0.42773237755459936,
-      "velocity" : 1.7109295102183972,
-      "acceleration" : 4.000000000000002,
+      "time" : 0.35684276742474663,
+      "velocity" : 1.4273710696989863,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6405304865329526,
+          "y" : -2.520230419613654
+        },
+        "rotation" : {
+          "radians" : -0.006233177930525208
+        }
+      },
+      "curvature" : -0.03877108765930074
+    }, {
+      "time" : 0.40065288496208495,
+      "velocity" : 1.6026115398483394,
+      "acceleration" : 3.9999999999999933,
       "pose" : {
         "translation" : {
-          "x" : 1.557566435449644,
-          "y" : -1.9807609539604982
+          "x" : 1.7069005447805807,
+          "y" : -2.5207311531334256
         },
         "rotation" : {
-          "radians" : -0.005316401490354412
+          "radians" : -0.008873618264835237
         }
       },
-      "curvature" : -0.024942408322112766
+      "curvature" : -0.04040754941441055
     }, {
-      "time" : 0.49025872163920814,
-      "velocity" : 1.9610348865568323,
+      "time" : 0.44090317601445284,
+      "velocity" : 1.7636127040578107,
       "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 1.6723610279150307,
-          "y" : -1.9815427598077804
+          "x" : 1.7746427433321514,
+          "y" : -2.5214250950984933
         },
         "rotation" : {
-          "radians" : -0.008351502650457828
+          "radians" : -0.011609841654107651
         }
       },
-      "curvature" : -0.027389037359448042
+      "curvature" : -0.04010473649023733
     }, {
-      "time" : 0.5438078316620709,
-      "velocity" : 2.1752313266482837,
-      "acceleration" : 4.000000000000004,
+      "time" : 0.4784875189105668,
+      "velocity" : 1.9139500756422665,
+      "acceleration" : 3.9999999999999907,
+      "pose" : {
+        "translation" : {
+          "x" : 1.843746311953339,
+          "y" : -2.522322105081429
+        },
+        "rotation" : {
+          "radians" : -0.014331201202338068
+        }
+      },
+      "curvature" : -0.03847972749251971
+    }, {
+      "time" : 0.5139493740375787,
+      "velocity" : 2.055797496150314,
+      "acceleration" : 3.999999999999994,
       "pose" : {
         "translation" : {
-          "x" : 1.7831023281846683,
-          "y" : -1.9826350762866696
+          "x" : 1.9141249898821115,
+          "y" : -2.5234242072328925
         },
         "rotation" : {
-          "radians" : -0.011352042966441073
+          "radians" : -0.016956454931120607
         }
       },
-      "curvature" : -0.026118374329521733
+      "curvature" : -0.03601472391776367
     }, {
-      "time" : 0.591065084130195,
-      "velocity" : 2.36426033652078,
-      "acceleration" : 3.9999999999999956,
+      "time" : 0.5476314929124355,
+      "velocity" : 2.190525971649741,
+      "acceleration" : 4.000000000000006,
       "pose" : {
         "translation" : {
-          "x" : 1.8903556638542796,
-          "y" : -1.983994595335389
+          "x" : 1.985625719290681,
+          "y" : -2.5247263738238246
         },
         "rotation" : {
-          "radians" : -0.013901156721571798
+          "radians" : -0.019428093132643293
         }
       },
-      "curvature" : -0.020693256448048484
+      "curvature" : -0.03305737128049612
     }, {
-      "time" : 0.633814678108913,
-      "velocity" : 2.535258712435652,
-      "acceleration" : 3.9999999999999916,
+      "time" : 0.5797531676739855,
+      "velocity" : 2.3190126706959413,
+      "acceleration" : 4.000000000000006,
       "pose" : {
         "translation" : {
-          "x" : 1.9950703586223426,
-          "y" : -1.9855485920954834
+          "x" : 2.0580373387474538,
+          "y" : -2.5262173087876363
         },
         "rotation" : {
-          "radians" : -0.015616242983820706
+          "radians" : -0.021706661066398916
         }
       },
-      "curvature" : -0.011492597860093103
+      "curvature" : -0.02983965013490699
     }, {
-      "time" : 0.6733634723933186,
-      "velocity" : 2.693453889573274,
-      "acceleration" : -4.0,
+      "time" : 0.6104540649254343,
+      "velocity" : 2.4418162597017363,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.0984517335891724,
-          "y" : -1.9872047305107117
+          "x" : 2.131099276678981,
+          "y" : -2.527880231262401
         },
         "rotation" : {
-          "radians" : -0.016221646042963344
+          "radians" : -0.023765711637107785
         }
       },
-      "curvature" : 0.0
+      "curvature" : -0.026501856917817767
+    }, {
+      "time" : 0.6398209489868676,
+      "velocity" : 2.5592837959474695,
+      "acceleration" : 3.999999999999994,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2045102448319085,
+          "y" : -2.529693659133045
+        },
+        "rotation" : {
+          "radians" : -0.025587608770040265
+        }
+      },
+      "curvature" : -0.02311472481731095
+    }, {
+      "time" : 0.6679049204495552,
+      "velocity" : 2.67161968179822,
+      "acceleration" : 2.6312398843848555,
+      "pose" : {
+        "translation" : {
+          "x" : 2.277936931734928,
+          "y" : -2.5316321925735394
+        },
+        "rotation" : {
+          "radians" : -0.027160172194753095
+        }
+      },
+      "curvature" : -0.019696900519105796
+    }, {
+      "time" : 0.6949126774005732,
+      "velocity" : 2.7426835690755107,
+      "acceleration" : -3.999999999999994,
+      "pose" : {
+        "translation" : {
+          "x" : 2.351022696160726,
+          "y" : -2.5336672975890906
+        },
+        "rotation" : {
+          "radians" : -0.02847404834663094
+        }
+      },
+      "curvature" : -0.01622720956565302
+    }, {
+      "time" : 0.7218404114156483,
+      "velocity" : 2.6349726330152103,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4233962605879356,
+          "y" : -2.5357680895583314
+        },
+        "rotation" : {
+          "radians" : -0.02952065953655597
+        }
+      },
+      "curvature" : -0.012652107561005304
     }, {
-      "time" : 0.7129122666777242,
-      "velocity" : 2.5352587124356516,
-      "acceleration" : -3.9999999999999956,
+      "time" : 0.7494857099529498,
+      "velocity" : 2.524391438866004,
+      "acceleration" : -3.999999999999994,
       "pose" : {
         "translation" : {
-          "x" : 2.201833108556002,
-          "y" : -1.98886086892594
+          "x" : 2.494680404663086,
+          "y" : -2.5379021167755127
         },
         "rotation" : {
-          "radians" : -0.015616242983820706
+          "radians" : -0.030290590291302242
         }
       },
-      "curvature" : 0.011492597860093103
+      "curvature" : -0.008889004344138316
     }, {
-      "time" : 0.7556618606564423,
-      "velocity" : 2.3642603365207795,
+      "time" : 0.7777916661649981,
+      "velocity" : 2.411167614017811,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.306547803324065,
-          "y" : -1.9904148656860343
+          "x" : 2.5645006586625527,
+          "y" : -2.540036143992694
         },
         "rotation" : {
-          "radians" : -0.013901156721571798
+          "radians" : -0.030772296527561225
         }
       },
-      "curvature" : 0.020693256448048484
+      "curvature" : -0.004826154357885299
     }, {
-      "time" : 0.8029191131245663,
-      "velocity" : 2.1752313266482832,
-      "acceleration" : -3.999999999999996,
+      "time" : 0.8066975299375645,
+      "velocity" : 2.2955441589275454,
+      "acceleration" : -4.000000000000007,
       "pose" : {
         "translation" : {
-          "x" : 2.4138011389936764,
-          "y" : -1.9917743847347538
+          "x" : 2.6324939969545085,
+          "y" : -2.542136935961935
         },
         "rotation" : {
-          "radians" : -0.011352042966441073
+          "radians" : -0.030951061958126948
         }
       },
-      "curvature" : 0.026118374329521733
+      "curvature" : -3.197882350940787E-4
     }, {
-      "time" : 0.8564682231474292,
-      "velocity" : 1.9610348865568321,
+      "time" : 0.8361410171738428,
+      "velocity" : 2.177770209982432,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.524542439263314,
-          "y" : -1.992866701213643
+          "x" : 2.698317531460873,
+          "y" : -2.544172040977486
+        },
+        "rotation" : {
+          "radians" : -0.030808177370938297
+        }
+      },
+      "curvature" : 0.004810664209664993
+    }, {
+      "time" : 0.8660614348369912,
+      "velocity" : 2.0580885393298387,
+      "acceleration" : -4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7616572051192634,
+          "y" : -2.5461105744179804
         },
         "rotation" : {
-          "radians" : -0.008351502650457828
+          "radians" : -0.030320390272959595
         }
       },
-      "curvature" : 0.027389037359448042
+      "curvature" : 0.010787761573423147
     }, {
-      "time" : 0.9189945672320379,
-      "velocity" : 1.7109295102183972,
+      "time" : 0.9271262518256638,
+      "velocity" : 1.8138292713751483,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.639337031728701,
-          "y" : -1.9936485070609251
+          "x" : 2.8798250574927806,
+          "y" : -2.549586924763389
         },
         "rotation" : {
-          "radians" : -0.005316401490354412
+          "radians" : -0.02819439651168064
         }
       },
-      "curvature" : 0.024942408322112766
+      "curvature" : 0.02638800187200745
     }, {
-      "time" : 0.9952957009831488,
-      "velocity" : 1.4057249752139536,
+      "time" : 0.989659585589217,
+      "velocity" : 1.5636959363209353,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9853920694440603,
+          "y" : -2.552380026318133
+        },
+        "rotation" : {
+          "radians" : -0.02431565149909576
+        }
+      },
+      "curvature" : 0.04885357015887642
+    }, {
+      "time" : 1.054053992446091,
+      "velocity" : 1.3061183088934392,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 3.077770434158083,
+          "y" : -2.554379138452532
+        },
+        "rotation" : {
+          "radians" : -0.018496813941660972
+        }
+      },
+      "curvature" : 0.07878050827674199
+    }, {
+      "time" : 1.1223816921391718,
+      "velocity" : 1.0328075101211158,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.758238249181886,
-          "y" : -1.9941157166103949
+          "x" : 3.1576682134182192,
+          "y" : -2.5555738139373716
         },
         "rotation" : {
-          "radians" : -0.0026512076017559455
+          "radians" : -0.01102726873603195
         }
       },
-      "curvature" : 0.019459649604384354
+      "curvature" : 0.10654928830990536
     }, {
-      "time" : 1.0971407902098755,
-      "velocity" : 0.9983446183070466,
+      "time" : 1.2022085804462013,
+      "velocity" : 0.7134999568929983,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.8806594309103275,
-          "y" : -1.9943132721903112
+          "x" : 3.2273675283886405,
+          "y" : -2.556078972294017
         },
         "rotation" : {
-          "radians" : -7.425481719022404E-4
+          "radians" : -0.003571825888394278
         }
       },
-      "curvature" : 0.011304718810550897
+      "curvature" : 0.09616404801082394
     }, {
-      "time" : 1.3467269447866372,
+      "time" : 1.3805835696694508,
       "velocity" : 0.0,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 3.0052459239959717,
-          "y" : -1.99434494972229
+          "x" : 3.2910027503967285,
+          "y" : -2.5561599731445312
         },
         "rotation" : {
           "radians" : 0.0
@@ -1876,11 +1932,11 @@
       "curvature" : 0.0
     } ],
     "pointList" : [ {
-      "x" : [ 1.191657543182373, 2.0, 0.0 ],
-      "y" : [ -1.9800645112991333, 0.0, 0.0 ]
+      "x" : [ 1.3858580589294434, 2.0, 0.0 ],
+      "y" : [ -2.519644260406494, 0.0, 0.0 ]
     }, {
-      "x" : [ 3.0052459239959717, 2.0, 0.0 ],
-      "y" : [ -1.99434494972229, 0.0, 0.0 ]
+      "x" : [ 3.2910027503967285, 1.0, 0.0 ],
+      "y" : [ -2.5561599731445312, 0.0, 0.0 ]
     } ],
     "rotations" : [ {
       "time" : 0.0,
@@ -1888,7 +1944,7 @@
         "radians" : -2.670353755551324
       }
     }, {
-      "time" : 1.2599999718368053,
+      "time" : 1.289999971166253,
       "rotation" : {
         "radians" : -2.684023492023121
       }
@@ -1901,7 +1957,7 @@
     "constraints" : [ ]
   }, {
     "type" : "script",
-    "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 20\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF",
+    "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 8.5\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF\nIntake.setIntakeSolState CLOSE",
     "closed" : false,
     "valid" : true,
     "sendableScript" : {
@@ -1909,7 +1965,7 @@
       "delay" : 0.0,
       "commands" : [ {
         "methodName" : "frc.subsystem.VisionManager.shootBalls",
-        "args" : [ "20" ],
+        "args" : [ "8.5" ],
         "argTypes" : [ "double" ],
         "reflection" : true
       }, {
@@ -1922,7 +1978,767 @@
         "args" : [ "OFF" ],
         "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
         "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "CLOSE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
       } ]
     }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2910027503967285,
+          "y" : -2.5561599731445312
+        },
+        "rotation" : {
+          "radians" : -0.2884106402522736
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.23642508906658902,
+      "velocity" : 0.9457003562663563,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3981667469918193,
+          "y" : -2.5879985818380646
+        },
+        "rotation" : {
+          "radians" : -0.2891712372126716
+        }
+      },
+      "curvature" : -5.874320584109886E-4
+    }, {
+      "time" : 0.3444359926496966,
+      "velocity" : 1.3777439705987866,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5184656394194462,
+          "y" : -2.6236786987501546
+        },
+        "rotation" : {
+          "radians" : -0.286576361816551
+        }
+      },
+      "curvature" : 0.04203266118630113
+    }, {
+      "time" : 0.3927805454968463,
+      "velocity" : 1.5711221819873853,
+      "acceleration" : 3.9999999999999942,
+      "pose" : {
+        "translation" : {
+          "x" : 3.586874350548449,
+          "y" : -2.643708721189732
+        },
+        "rotation" : {
+          "radians" : -0.2828782585831171
+        }
+      },
+      "curvature" : 0.06060524122763304
+    }, {
+      "time" : 0.43985280368645924,
+      "velocity" : 1.7594112147458367,
+      "acceleration" : 3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 3.662202759596539,
+          "y" : -2.665394846008212
+        },
+        "rotation" : {
+          "radians" : -0.277565073334469
+        }
+      },
+      "curvature" : 0.07384725863707324
+    }, {
+      "time" : 0.4864477060306216,
+      "velocity" : 1.9457908241224862,
+      "acceleration" : 3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.745298805531398,
+          "y" : -2.688772057520943
+        },
+        "rotation" : {
+          "radians" : -0.2707950380725529
+        }
+      },
+      "curvature" : 0.08213625833510316
+    }, {
+      "time" : 0.5329979230669721,
+      "velocity" : 2.131991692267888,
+      "acceleration" : 4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8368513446766883,
+          "y" : -2.7137960128020495
+        },
+        "rotation" : {
+          "radians" : -0.2627587722278301
+        }
+      },
+      "curvature" : 0.08660282922194644
+    }, {
+      "time" : 0.5797279435183833,
+      "velocity" : 2.3189117740735328,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9373990498514004,
+          "y" : -2.740351675968121
+        },
+        "rotation" : {
+          "radians" : -0.25363830846069757
+        }
+      },
+      "curvature" : 0.08841580171796991
+    }, {
+      "time" : 0.6267362610549605,
+      "velocity" : 2.5069450442198415,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.047339309509198,
+          "y" : -2.7682619524618985
+        },
+        "rotation" : {
+          "radians" : -0.2435910468434367
+        }
+      },
+      "curvature" : 0.0885183696531404
+    }, {
+      "time" : 0.6740431507892504,
+      "velocity" : 2.696172603157001,
+      "acceleration" : 3.9999999999999862,
+      "pose" : {
+        "translation" : {
+          "x" : 4.166937126877762,
+          "y" : -2.7972963233359636
+        },
+        "rotation" : {
+          "radians" : -0.2327470040418315
+        }
+      },
+      "curvature" : 0.08758788738466654
+    }, {
+      "time" : 0.6978010763245834,
+      "velocity" : 2.791204305298333,
+      "acceleration" : 3.999999999999974,
+      "pose" : {
+        "translation" : {
+          "x" : 4.230406424149577,
+          "y" : -2.8121505236061024
+        },
+        "rotation" : {
+          "radians" : -0.22706006402416112
+        }
+      },
+      "curvature" : 0.08688404973771191
+    }, {
+      "time" : 0.7216202590268884,
+      "velocity" : 2.8864810361075524,
+      "acceleration" : 3.9999999999999747,
+      "pose" : {
+        "translation" : {
+          "x" : 4.29633401909814,
+          "y" : -2.827179479536426
+        },
+        "rotation" : {
+          "radians" : -0.22121199588447016
+        }
+      },
+      "curvature" : 0.08607766603782731
+    }, {
+      "time" : 0.74549257464285,
+      "velocity" : 2.9819702985713983,
+      "acceleration" : 0.7442098400976622,
+      "pose" : {
+        "translation" : {
+          "x" : 4.364719766680094,
+          "y" : -2.8423431956032283
+        },
+        "rotation" : {
+          "radians" : -0.21521310387858186
+        }
+      },
+      "curvature" : 0.08519994915603576
+    }, {
+      "time" : 0.7697192113482115,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.1864982661111718E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.435556916364085,
+          "y" : -2.8575999561866112
+        },
+        "rotation" : {
+          "radians" : -0.2090728501840574
+        }
+      },
+      "curvature" : 0.08427499675849662
+    }, {
+      "time" : 0.7946715843679995,
+      "velocity" : 3.0,
+      "acceleration" : -1.1500748104073835E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.508832390228868,
+          "y" : -2.8729065953918482
+        },
+        "rotation" : {
+          "radians" : -0.2028000166117804
+        }
+      },
+      "curvature" : 0.08332106007566421
+    }, {
+      "time" : 0.8204142117652993,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.1162447712385717E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.584527061061408,
+          "y" : -2.88821876687075
+        },
+        "rotation" : {
+          "radians" : -0.19640285659122264
+        }
+      },
+      "curvature" : 0.08235160626948937
+    }, {
+      "time" : 0.8469370211760767,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.6626160304549895,
+          "y" : -2.9034912136430293
+        },
+        "rotation" : {
+          "radians" : -0.18988923719000408
+        }
+      },
+      "curvature" : 0.0813761949653504
+    }, {
+      "time" : 0.874228259006809,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.743068906907318,
+          "y" : -2.9186780379176653
+        },
+        "rotation" : {
+          "radians" : -0.18326677164075206
+        }
+      },
+      "curvature" : 0.08040119291556454
+    }, {
+      "time" : 0.9022745981913911,
+      "velocity" : 3.0,
+      "acceleration" : -1.0284611572537565E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.825850083918625,
+          "y" : -2.9337329709142717
+        },
+        "rotation" : {
+          "radians" : -0.17654294326329226
+        }
+      },
+      "curvature" : 0.07943035076297603
+    }, {
+      "time" : 0.9310612442543232,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.0032243113209958E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.910919018089771,
+          "y" : -2.9486096426844597
+        },
+        "rotation" : {
+          "radians" : -0.16972522188764874
+        }
+      },
+      "curvature" : 0.07846526410378302
+    }, {
+      "time" : 0.9605720395861806,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.998230507220355,
+          "y" : -2.9632618519332032
+        },
+        "rotation" : {
+          "radians" : -0.16282117397282284
+        }
+      },
+      "curvature" : 0.07750573849274361
+    }, {
+      "time" : 0.990789565834994,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.087734968406814,
+          "y" : -2.9776438358402046
+        },
+        "rotation" : {
+          "radians" : -0.15583856762569545
+        }
+      },
+      "curvature" : 0.07655007526138907
+    }, {
+      "time" : 1.0216952443158518,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.179378716140528,
+          "y" : -2.991710539881261
+        },
+        "rotation" : {
+          "radians" : -0.14878547368600106
+        }
+      },
+      "curvature" : 0.07559529234150263
+    }, {
+      "time" : 1.0532694343409463,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.273104240405928,
+          "y" : -3.005417887649628
+        },
+        "rotation" : {
+          "radians" : -0.14167036397863442
+        }
+      },
+      "curvature" : 0.07463729185441531
+    }, {
+      "time" : 1.0854915293736447,
+      "velocity" : 3.0,
+      "acceleration" : -2.7038611726562314E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 5.368850484778598,
+          "y" : -3.0187230506773854
+        },
+        "rotation" : {
+          "radians" : -0.13450220775752084
+        }
+      },
+      "curvature" : 0.07367098410111733
+    }, {
+      "time" : 1.1183400509132173,
+      "velocity" : 2.999999999999999,
+      "acceleration" : 2.6550284696523992E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 5.466553124523379,
+          "y" : -3.031584718256802
+        },
+        "rotation" : {
+          "radians" : -0.1272905672847317
+        }
+      },
+      "curvature" : 0.0726903757761293
+    }, {
+      "time" : 1.151792740021911,
+      "velocity" : 3.0,
+      "acceleration" : -8.698956559016603E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 5.566144844692476,
+          "y" : -3.0439633672617026
+        },
+        "rotation" : {
+          "radians" : -0.1200456934097773
+        }
+      },
+      "curvature" : 0.07168862870971174
+    }, {
+      "time" : 1.1858266464134202,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.558717775961129E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 5.66755561822356,
+          "y" : -3.0558215319688316
+        },
+        "rotation" : {
+          "radians" : -0.11277862194006867
+        }
+      },
+      "curvature" : 0.0706580941863296
+    }, {
+      "time" : 1.2204182150319014,
+      "velocity" : 3.0,
+      "acceleration" : -8.428702249529382E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 5.770712984037872,
+          "y" : -3.067124073879218
+        },
+        "rotation" : {
+          "radians" : -0.10550127152586215
+        }
+      },
+      "curvature" : 0.06959032685672162
+    }, {
+      "time" : 1.2555433700639118,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.308289645985856E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 5.875542325138333,
+          "y" : -3.0778384515395416
+        },
+        "rotation" : {
+          "radians" : -0.09822654372203397
+        }
+      },
+      "curvature" : 0.06847608142197704
+    }, {
+      "time" : 1.2911775963425318,
+      "velocity" : 3.0,
+      "acceleration" : -8.19691056891909E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 5.9819671467076425,
+          "y" : -3.0879349903634985
+        },
+        "rotation" : {
+          "radians" : -0.0909684258345216
+        }
+      },
+      "curvature" : 0.06730529458972327
+    }, {
+      "time" : 1.327296018124022,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.089909354206384,
+          "y" : -3.097387152453166
+        },
+        "rotation" : {
+          "radians" : -0.08374209711050397
+        }
+      },
+      "curvature" : 0.06606705425850384
+    }, {
+      "time" : 1.3638734752432662,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 7.999202988055673E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 6.199289531471132,
+          "y" : -3.1061718064203667
+        },
+        "rotation" : {
+          "radians" : -0.07656403878726785
+        }
+      },
+      "curvature" : 0.06474955745525926
+    }, {
+      "time" : 1.400884596685671,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.310027218812557,
+          "y" : -3.114269497208035
+        },
+        "rotation" : {
+          "radians" : -0.06945214847377139
+        }
+      },
+      "curvature" : 0.06334005821608486
+    }, {
+      "time" : 1.438303871650907,
+      "velocity" : 3.0,
+      "acceleration" : -7.831878599476166E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 6.422041191113526,
+          "y" : -3.1216647159115825
+        },
+        "rotation" : {
+          "radians" : -0.06242585929932353
+        }
+      },
+      "curvature" : 0.06182480635063325
+    }, {
+      "time" : 1.4761057182287096,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 7.758609490328921E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 6.535249735927209,
+          "y" : -3.1283461696002632
+        },
+        "rotation" : {
+          "radians" : -0.055506264223218596
+        }
+      },
+      "curvature" : 0.06018897785906844
+    }, {
+      "time" : 1.5142645498598688,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.6495709315751865,
+          "y" : -3.1343070511385376
+        },
+        "rotation" : {
+          "radians" : -0.04871624585462448
+        }
+      },
+      "curvature" : 0.05841659767581544
+    }, {
+      "time" : 1.5527548398175375,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.76492292524555,
+          "y" : -3.1395453090074383
+        },
+        "rotation" : {
+          "radians" : -0.0420806120796796
+        }
+      },
+      "curvature" : 0.05649045540029702
+    }, {
+      "time" : 1.5915511840162078,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.881224211091007,
+          "y" : -3.144063917125937
+        },
+        "rotation" : {
+          "radians" : -0.03562623772763141
+        }
+      },
+      "curvature" : 0.05439201475116482
+    }, {
+      "time" : 1.6306283625393438,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.998393908326989,
+          "y" : -3.147871144672308
+        },
+        "rotation" : {
+          "radians" : -0.029382212423449006
+        }
+      },
+      "curvature" : 0.05210131766377214
+    }, {
+      "time" : 1.6699614003730316,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 7.116352039329751,
+          "y" : -3.150980825905494
+        },
+        "rotation" : {
+          "radians" : -0.02337999466219851
+        }
+      },
+      "curvature" : 0.04959688426535946
+    }, {
+      "time" : 1.7095256279435234,
+      "velocity" : 3.0,
+      "acceleration" : -1.1557888191446533,
+      "pose" : {
+        "translation" : {
+          "x" : 7.235019807734481,
+          "y" : -3.1534126299864695
+        },
+        "rotation" : {
+          "radians" : -0.017653571989563004
+        }
+      },
+      "curvature" : 0.04685561044273053
+    }, {
+      "time" : 1.749606195370589,
+      "velocity" : 2.953675328302824,
+      "acceleration" : -4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 7.354319876533402,
+          "y" : -3.1551923307996104
+        },
+        "rotation" : {
+          "radians" : -0.012239626968958514
+        }
+      },
+      "curvature" : 0.04385266540863671
+    }, {
+      "time" : 1.791367879808199,
+      "velocity" : 2.786628590552384,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 7.4741766461738735,
+          "y" : -3.1563520767740556
+        },
+        "rotation" : {
+          "radians" : -0.007177708340391282
+        }
+      },
+      "curvature" : 0.040561392634885035
+    }, {
+      "time" : 1.8359816679429726,
+      "velocity" : 2.6081734380132895,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 7.594516532656503,
+          "y" : -3.156930660705073
+        },
+        "rotation" : {
+          "radians" : -0.002510406406084059
+        }
+      },
+      "curvature" : 0.036953218827764284
+    }, {
+      "time" : 1.8840509532237966,
+      "velocity" : 2.415896296889994,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 7.715268245633247,
+          "y" : -3.1569737895754244
+        },
+        "rotation" : {
+          "radians" : 0.0017164688167075213
+        }
+      },
+      "curvature" : 0.03299757736886057
+    }, {
+      "time" : 1.9364483118057918,
+      "velocity" : 2.206306862562013,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 7.836363066505513,
+          "y" : -3.156534354376733
+        },
+        "rotation" : {
+          "radians" : 0.005453708791556059
+        }
+      },
+      "curvature" : 0.028661854946767117
+    }, {
+      "time" : 1.9945178704913586,
+      "velocity" : 1.9740286278197459,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 7.957735126522266,
+          "y" : -3.1556726999308466
+        },
+        "rotation" : {
+          "radians" : 0.008648529987311705
+        }
+      },
+      "curvature" : 0.023911373099727667
+    }, {
+      "time" : 2.060528833894078,
+      "velocity" : 1.7099847742088685,
+      "acceleration" : -3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 8.079321684878137,
+          "y" : -3.1544568947112026
+        },
+        "rotation" : {
+          "radians" : 0.011244403839107409
+        }
+      },
+      "curvature" : 0.018709420234176016
+    }, {
+      "time" : 2.1389153715933484,
+      "velocity" : 1.3964386234117863,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 8.201063406811521,
+          "y" : -3.1529630006641938
+        },
+        "rotation" : {
+          "radians" : 0.013180898713490029
+        }
+      },
+      "curvature" : 0.013017354553867993
+    }, {
+      "time" : 2.2411422808075967,
+      "velocity" : 0.9875309865547927,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 8.322904641702685,
+          "y" : -3.1512753430305347
+        },
+        "rotation" : {
+          "radians" : 0.01439354131558399
+        }
+      },
+      "curvature" : 0.0067948044075592335
+    }, {
+      "time" : 2.488025027446295,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 8.444793701171875,
+          "y" : -3.149486780166626
+        },
+        "rotation" : {
+          "radians" : 0.014813707347434221
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 3.2910027503967285, 1.676727533340454, 0.0 ],
+      "y" : [ -2.5561599731445312, -0.49745607376098633, 0.0 ]
+    }, {
+      "x" : [ 8.444793701171875, 7.801394462585449, 0.0 ],
+      "y" : [ -3.149486780166626, 0.11557602882385254, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.0
+      }
+    }, {
+      "time" : 2.3999999463558197,
+      "rotation" : {
+        "radians" : 0.0
+      }
+    } ],
+    "reversed" : false,
+    "color" : 88.23529,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
   } ]
 }
\ No newline at end of file
diff --git a/src/main/deploy/autos/blue/buddyautoleftsteal.json b/src/main/deploy/autos/blue/buddyautoleftsteal.json
new file mode 100644
index 00000000..5264679f
--- /dev/null
+++ b/src/main/deploy/autos/blue/buddyautoleftsteal.json
@@ -0,0 +1,3806 @@
+{
+  "autonomousSteps" : [ {
+    "type" : "script",
+    "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON\nsleep 1500",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "OPEN" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "INTAKE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "ON" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "sleep",
+        "args" : [ "1500" ],
+        "argTypes" : [ "long" ],
+        "reflection" : false
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 6.979461669921875,
+          "y" : 0.4386838674545288
+        },
+        "rotation" : {
+          "radians" : 2.4670512351406453
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.2218186293424793,
+      "velocity" : 0.8872745173699175,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 6.902607691762569,
+          "y" : 0.5001444677761242
+        },
+        "rotation" : {
+          "radians" : 2.4669815013720786
+        }
+      },
+      "curvature" : -0.001770057444385252
+    }, {
+      "time" : 0.3128823124020749,
+      "velocity" : 1.2515292496083,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 6.826564423769014,
+          "y" : 0.5609803238967288
+        },
+        "rotation" : {
+          "radians" : 2.46664012448608
+        }
+      },
+      "curvature" : -0.005584153832447457
+    }, {
+      "time" : 0.38171187830481973,
+      "velocity" : 1.526847513219279,
+      "acceleration" : 3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 6.751933529499183,
+          "y" : 0.6207539377647322
+        },
+        "rotation" : {
+          "radians" : 2.465845563577437
+        }
+      },
+      "curvature" : -0.011384177747344891
+    }, {
+      "time" : 0.4386276460816924,
+      "velocity" : 1.7545105843267694,
+      "acceleration" : 3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 6.679111945442855,
+          "y" : 0.6792085023480467
+        },
+        "rotation" : {
+          "radians" : 2.4644380166116457
+        }
+      },
+      "curvature" : -0.019103896941460693
+    }, {
+      "time" : 0.4876979714419739,
+      "velocity" : 1.9507918857678952,
+      "acceleration" : 3.9999999999999925,
+      "pose" : {
+        "translation" : {
+          "x" : 6.608317537175935,
+          "y" : 0.7362419663763262
+        },
+        "rotation" : {
+          "radians" : 2.4622863748597443
+        }
+      },
+      "curvature" : -0.028529142584546568
+    }, {
+      "time" : 0.5310864114411233,
+      "velocity" : 2.1243456457644925,
+      "acceleration" : 4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 6.539614755514776,
+          "y" : 0.7918810990831844
+        },
+        "rotation" : {
+          "radians" : 2.459302059694302
+        }
+      },
+      "curvature" : -0.03918060333359783
+    }, {
+      "time" : 0.5701494584010045,
+      "velocity" : 2.2805978336040176,
+      "acceleration" : -0.47832492111012465,
+      "pose" : {
+        "translation" : {
+          "x" : 6.472940292670501,
+          "y" : 0.8462555549484136
+        },
+        "rotation" : {
+          "radians" : 2.455456663103906
+        }
+      },
+      "curvature" : -0.05024047921606022
+    }, {
+      "time" : 0.6070915310824326,
+      "velocity" : 2.262927519603029,
+      "acceleration" : -4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 6.40812873840332,
+          "y" : 0.8995719384402037
+        },
+        "rotation" : {
+          "radians" : 2.4507991475637327
+        }
+      },
+      "curvature" : -0.06056066057312981
+    }, {
+      "time" : 0.6446468880225168,
+      "velocity" : 2.1127060918426923,
+      "acceleration" : -4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 6.3449382361768585,
+          "y" : 0.95208786875736
+        },
+        "rotation" : {
+          "radians" : 2.445467378655361
+        }
+      },
+      "curvature" : -0.06878202666638442
+    }, {
+      "time" : 0.6843932978096723,
+      "velocity" : 1.9537204526940704,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.28307613931247,
+          "y" : 1.0040860445715225
+        },
+        "rotation" : {
+          "radians" : 2.4396891056204377
+        }
+      },
+      "curvature" : -0.07355656872184071
+    }, {
+      "time" : 0.7271558960672502,
+      "velocity" : 1.782670059663759,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.222224667143564,
+          "y" : 1.0558483087693844
+        },
+        "rotation" : {
+          "radians" : 2.4337699158796475
+        }
+      },
+      "curvature" : -0.07380896893317555
+    }, {
+      "time" : 0.7741602914447601,
+      "velocity" : 1.5946524781537195,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.162066561169922,
+          "y" : 1.1076297131949104
+        },
+        "rotation" : {
+          "radians" : 2.428069867169106
+        }
+      },
+      "curvature" : -0.06893533742465202
+    }, {
+      "time" : 0.8273894003721558,
+      "velocity" : 1.3817360424441365,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 6.1023107412120225,
+          "y" : 1.1596325833915557
+        },
+        "rotation" : {
+          "radians" : 2.422974921973857
+        }
+      },
+      "curvature" : -0.058846523386872546
+    }, {
+      "time" : 0.8905738529525908,
+      "velocity" : 1.1289982321223961,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 6.042717961565359,
+          "y" : 1.2119805833444843
+        },
+        "rotation" : {
+          "radians" : 2.418872019409643
+        }
+      },
+      "curvature" : -0.043821739722789785
+    }, {
+      "time" : 0.9731110220523553,
+      "velocity" : 0.7988495557233383,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.9831264671547615,
+          "y" : 1.2646927802227879
+        },
+        "rotation" : {
+          "radians" : 2.4161366104589406
+        }
+      },
+      "curvature" : -0.02420493001126614
+    }, {
+      "time" : 1.1728234109831899,
+      "velocity" : 0.0,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.923477649688721,
+          "y" : 1.317657709121704
+        },
+        "rotation" : {
+          "radians" : 2.4151395686141304
+        }
+      },
+      "curvature" : -0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 6.979461669921875, -1.232039451599121, 0.0 ],
+      "y" : [ 0.4386838674545288, 0.9852284789085388, 0.0 ]
+    }, {
+      "x" : [ 5.923477649688721, -0.9546575546264648, 0.0 ],
+      "y" : [ 1.317657709121704, 0.8482611179351807, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.7759904509715376
+      }
+    }, {
+      "time" : 1.0799999758601189,
+      "rotation" : {
+        "radians" : 2.3681701999637936
+      }
+    } ],
+    "reversed" : false,
+    "color" : 209.88235,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "VisionManager.shootBalls 13",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.VisionManager.shootBalls",
+        "args" : [ "13" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.923477649688721,
+          "y" : 1.317657709121704
+        },
+        "rotation" : {
+          "radians" : 2.4280629259945457
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.1916862579332135,
+      "velocity" : 0.766745031732854,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.8679256060540865,
+          "y" : 1.3657653522359396
+        },
+        "rotation" : {
+          "radians" : 2.427546282399253
+        }
+      },
+      "curvature" : -0.013552982296289647
+    }, {
+      "time" : 0.26883282720714063,
+      "velocity" : 1.0753313088285628,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.814256962185027,
+          "y" : 1.4123322981758974
+        },
+        "rotation" : {
+          "radians" : 2.426170125310746
+        }
+      },
+      "curvature" : -0.025078420554477936
+    }, {
+      "time" : 0.3253598004362359,
+      "velocity" : 1.3014392017449439,
+      "acceleration" : 3.999999999999993,
+      "pose" : {
+        "translation" : {
+          "x" : 5.763593902279354,
+          "y" : 1.4564440353524333
+        },
+        "rotation" : {
+          "radians" : 2.424133609238276
+        }
+      },
+      "curvature" : -0.035413495196781865
+    }, {
+      "time" : 0.37047014804781875,
+      "velocity" : 1.4818805921912748,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.716341427527368,
+          "y" : 1.4977757879532874
+        },
+        "rotation" : {
+          "radians" : 2.4216494598565745
+        }
+      },
+      "curvature" : -0.043163158740255655
+    }, {
+      "time" : 0.44071473447231035,
+      "velocity" : 1.7628589378892412,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.630849667446455,
+          "y" : 1.57313263497781
+        },
+        "rotation" : {
+          "radians" : 2.4166846345884783
+        }
+      },
+      "curvature" : -0.03766210226111181
+    }, {
+      "time" : 0.4973489273055233,
+      "velocity" : 1.989395709222093,
+      "acceleration" : -1.536593152344523,
+      "pose" : {
+        "translation" : {
+          "x" : 5.551411211490631,
+          "y" : 1.643696241080761
+        },
+        "rotation" : {
+          "radians" : 2.4146239525620357
+        }
+      },
+      "curvature" : 0.002386794122151864
+    }, {
+      "time" : 0.5545817819308377,
+      "velocity" : 1.9014520967157054,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 5.468152332963655,
+          "y" : 1.7176221262780018
+        },
+        "rotation" : {
+          "radians" : 2.416957846275337
+        }
+      },
+      "curvature" : 0.03338570540861069
+    }, {
+      "time" : 0.5881329413084252,
+      "velocity" : 1.7672474592053555,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.422027663326844,
+          "y" : 1.7583682663707805
+        },
+        "rotation" : {
+          "radians" : 2.419124204295016
+        }
+      },
+      "curvature" : 0.03568476607637792
+    }, {
+      "time" : 0.6278550666121283,
+      "velocity" : 1.6083589579905428,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.3716818848624825,
+          "y" : 1.8026411016471684
+        },
+        "rotation" : {
+          "radians" : 2.42141292546337
+        }
+      },
+      "curvature" : 0.03192840803852446
+    }, {
+      "time" : 0.67622301229786,
+      "velocity" : 1.4148871752476162,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.316670433480795,
+          "y" : 1.850801377645439
+        },
+        "rotation" : {
+          "radians" : 2.4235110036015075
+        }
+      },
+      "curvature" : 0.025280758123603746
+    }, {
+      "time" : 0.7373987455586821,
+      "velocity" : 1.1701842422043276,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.257077135815052,
+          "y" : 1.9027724422048777
+        },
+        "rotation" : {
+          "radians" : 2.4252082067772336
+        }
+      },
+      "curvature" : 0.017656926445273908
+    }, {
+      "time" : 0.8212616808414924,
+      "velocity" : 0.8347325010730865,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.193638766594631,
+          "y" : 1.9579375321181942
+        },
+        "rotation" : {
+          "radians" : 2.4263532239110557
+        }
+      },
+      "curvature" : 0.009537673600406624
+    }, {
+      "time" : 1.029944806109764,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.127869606018066,
+          "y" : 2.0150370597839355
+        },
+        "rotation" : {
+          "radians" : 2.4267848465547592
+        }
+      },
+      "curvature" : -0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.923477649688721, -0.8946475982666016, 0.0 ],
+      "y" : [ 1.317657709121704, 0.7744828462600708, 0.0 ]
+    }, {
+      "x" : [ 5.127869606018066, -1.0593681335449219, 0.0 ],
+      "y" : [ 2.0150370597839355, 0.9194501638412476, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.356194490192345
+      }
+    }, {
+      "time" : 0.9399999789893627,
+      "rotation" : {
+        "radians" : 2.4532338595176353
+      }
+    } ],
+    "reversed" : false,
+    "color" : 154.35294,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "VisionManager.shootBalls 8\nDrive.turnToAngle 40",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.VisionManager.shootBalls",
+        "args" : [ "8" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Drive.turnToAngle",
+        "args" : [ "40" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.127869606018066,
+          "y" : 2.0150370597839355
+        },
+        "rotation" : {
+          "radians" : 0.9676638940223135
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.23907872328411964,
+      "velocity" : 0.9563148931364785,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.192705269680573,
+          "y" : 2.109189995806446
+        },
+        "rotation" : {
+          "radians" : 0.9679091836027879
+        }
+      },
+      "curvature" : 0.004044048851551997
+    }, {
+      "time" : 0.3327971604973431,
+      "velocity" : 1.3311886419893724,
+      "acceleration" : 3.9999999999999907,
+      "pose" : {
+        "translation" : {
+          "x" : 5.253460748921498,
+          "y" : 2.197499692440033
+        },
+        "rotation" : {
+          "radians" : 0.9685084969782757
+        }
+      },
+      "curvature" : 0.007076805090390186
+    }, {
+      "time" : 0.39816082847441153,
+      "velocity" : 1.5926433138976455,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.307566488200337,
+          "y" : 2.2762623392827663
+        },
+        "rotation" : {
+          "radians" : 0.9692958679676649
+        }
+      },
+      "curvature" : 0.009223838838357203
+    }, {
+      "time" : 0.44663324990958686,
+      "velocity" : 1.7865329996383468,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 5.353885251563042,
+          "y" : 2.343804362230003
+        },
+        "rotation" : {
+          "radians" : 0.9700562639818262
+        }
+      },
+      "curvature" : 0.008519844053710474
+    }, {
+      "time" : 0.5116917209128728,
+      "velocity" : 2.0467668836514905,
+      "acceleration" : 1.992648464590459,
+      "pose" : {
+        "translation" : {
+          "x" : 5.424351285982993,
+          "y" : 2.4466790923615918
+        },
+        "rotation" : {
+          "radians" : 0.9696596521756633
+        }
+      },
+      "curvature" : -0.030593782175814987
+    }, {
+      "time" : 0.5546990819250267,
+      "velocity" : 2.1324654355384465,
+      "acceleration" : -3.9999999999999925,
+      "pose" : {
+        "translation" : {
+          "x" : 5.475393638014793,
+          "y" : 2.5206459313631058
+        },
+        "rotation" : {
+          "radians" : 0.9621486944770179
+        }
+      },
+      "curvature" : -0.13823598332150094
+    }, {
+      "time" : 0.5969447671310003,
+      "velocity" : 1.9634826947145523,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.5252832076948835,
+          "y" : 2.591331197647378
+        },
+        "rotation" : {
+          "radians" : 0.9510157851137995
+        }
+      },
+      "curvature" : -0.09101992339026437
+    }, {
+      "time" : 0.6595418386244202,
+      "velocity" : 1.7130944087408728,
+      "acceleration" : -3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 5.592436509672552,
+          "y" : 2.6847757240757346
+        },
+        "rotation" : {
+          "radians" : 0.9456880198624937
+        }
+      },
+      "curvature" : -0.018285197310074868
+    }, {
+      "time" : 0.7058887043411002,
+      "velocity" : 1.527706945874153,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.636414063088068,
+          "y" : 2.745653131905783
+        },
+        "rotation" : {
+          "radians" : 0.9448592549816716
+        }
+      },
+      "curvature" : -0.005896965233362419
+    }, {
+      "time" : 0.768439061104125,
+      "velocity" : 1.2775055188220539,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 5.687825173619785,
+          "y" : 2.8167451025219634
+        },
+        "rotation" : {
+          "radians" : 0.9445964731587643
+        }
+      },
+      "curvature" : -0.001026058794484897
+    }, {
+      "time" : 0.8582844674175891,
+      "velocity" : 0.9181238935681972,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.745632715359989,
+          "y" : 2.896663040502972
+        },
+        "rotation" : {
+          "radians" : 0.9445853727793603
+        }
+      },
+      "curvature" : 3.9777751022351305E-4
+    }, {
+      "time" : 1.0878154408096383,
+      "velocity" : 0.0,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.807385444641113,
+          "y" : 2.9820399284362793
+        },
+        "rotation" : {
+          "radians" : 0.9446200461231519
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.127869606018066, 1.0498194694519043, 0.0 ],
+      "y" : [ 2.0150370597839355, 1.5242493152618408, 0.0 ]
+    }, {
+      "x" : [ 5.807385444641113, 1.0001163482666016, 0.0 ],
+      "y" : [ 2.9820399284362793, 1.3827617168426514, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.9380478872771981
+      }
+    }, {
+      "time" : 0.9999999776482582,
+      "rotation" : {
+        "radians" : 0.9272952180016122
+      }
+    } ],
+    "reversed" : false,
+    "color" : 59.52942,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@time 1.4\nIntake.setWantedIntakeState EJECT",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 1.4,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "EJECT" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.807385444641113,
+          "y" : 2.9820399284362793
+        },
+        "rotation" : {
+          "radians" : -3.1077984471976654
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.2180776573035808,
+      "velocity" : 0.8723106292143229,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.712328077295783,
+          "y" : 2.978708433225936
+        },
+        "rotation" : {
+          "radians" : -3.1041624869647366
+        }
+      },
+      "curvature" : 0.07273634738038368
+    }, {
+      "time" : 0.3111557355200276,
+      "velocity" : 1.2446229420801103,
+      "acceleration" : 3.9999999999999956,
+      "pose" : {
+        "translation" : {
+          "x" : 5.613894489667587,
+          "y" : 2.974581701005377
+        },
+        "rotation" : {
+          "radians" : -3.094472230094453
+        }
+      },
+      "curvature" : 0.11857740286674891
+    }, {
+      "time" : 0.38620859133114593,
+      "velocity" : 1.5448343653245833,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.509367328174033,
+          "y" : 2.9689573476464197
+        },
+        "rotation" : {
+          "radians" : -3.0808888420166256
+        }
+      },
+      "curvature" : 0.1367542913886183
+    }, {
+      "time" : 0.4534555009735825,
+      "velocity" : 1.8138220038943296,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 5.39670240160558,
+          "y" : 2.961229747164907
+        },
+        "rotation" : {
+          "radians" : -3.065345218180051
+        }
+      },
+      "curvature" : 0.13629877278960334
+    }, {
+      "time" : 0.5166742966413302,
+      "velocity" : 2.0666971865653205,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 5.274478519695425,
+          "y" : 2.950885042220694
+        },
+        "rotation" : {
+          "radians" : -3.0491647636938963
+        }
+      },
+      "curvature" : 0.1267933963841827
+    }, {
+      "time" : 0.5473532125193565,
+      "velocity" : 2.189412850077426,
+      "acceleration" : 4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 5.209495855934646,
+          "y" : 2.9445945602601076
+        },
+        "rotation" : {
+          "radians" : -3.041082387571697
+        }
+      },
+      "curvature" : 0.12079034959194605
+    }, {
+      "time" : 0.5775859258575893,
+      "velocity" : 2.310343703430357,
+      "acceleration" : 3.999999999999994,
+      "pose" : {
+        "translation" : {
+          "x" : 5.141847331689291,
+          "y" : 2.9374961546176337
+        },
+        "rotation" : {
+          "radians" : -3.0330761640829413
+        }
+      },
+      "curvature" : 0.11467075614396673
+    }, {
+      "time" : 0.607452738767687,
+      "velocity" : 2.429810955070748,
+      "acceleration" : 4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 5.071508179679264,
+          "y" : 2.929549181170158
+        },
+        "rotation" : {
+          "radians" : -3.025171418955131
+        }
+      },
+      "curvature" : 0.10876301194249897
+    }, {
+      "time" : 0.6370065753804001,
+      "velocity" : 2.5480263015216003,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.998483164915225,
+          "y" : 2.920717795803565
+        },
+        "rotation" : {
+          "radians" : -3.017377231431235
+        }
+      },
+      "curvature" : 0.10327528635802412
+    }, {
+      "time" : 0.6662799728523379,
+      "velocity" : 2.6651198914093515,
+      "acceleration" : 3.9999999999999942,
+      "pose" : {
+        "translation" : {
+          "x" : 4.922805017153899,
+          "y" : 2.910970798490861
+        },
+        "rotation" : {
+          "radians" : -3.0096904819137515
+        }
+      },
+      "curvature" : 0.09832969802645626
+    }, {
+      "time" : 0.6952901141770801,
+      "velocity" : 2.7811604567083203,
+      "acceleration" : 3.9999999999999836,
+      "pose" : {
+        "translation" : {
+          "x" : 4.844532863353379,
+          "y" : 2.9002814773702994
+        },
+        "rotation" : {
+          "radians" : -3.0020990455222543
+        }
+      },
+      "curvature" : 0.09398961737092937
+    }, {
+      "time" : 0.724042537599304,
+      "velocity" : 2.8961701503972157,
+      "acceleration" : 3.6380002628705683,
+      "pose" : {
+        "translation" : {
+          "x" : 4.763750660128433,
+          "y" : 2.888627452823507
+        },
+        "rotation" : {
+          "radians" : -2.994584224283115
+        }
+      },
+      "curvature" : 0.09028031417064307
+    }, {
+      "time" : 0.7525829010133674,
+      "velocity" : 3.0,
+      "acceleration" : -1.0263248734009206E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.680565626205809,
+          "y" : 2.8759905215536072
+        },
+        "rotation" : {
+          "radians" : -2.9871225391208203
+        }
+      },
+      "curvature" : 0.08720396777763022
+    }, {
+      "time" : 0.7814294661611385,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.000209901006119E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.595106674879542,
+          "y" : 2.8623565006633434
+        },
+        "rotation" : {
+          "radians" : -2.979687003342993
+        }
+      },
+      "curvature" : 0.08475025940246926
+    }, {
+      "time" : 0.8110292004704637,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.507522846466259,
+          "y" : 2.847715071733205
+        },
+        "rotation" : {
+          "radians" : -2.9722479862521185
+        }
+      },
+      "curvature" : 0.08290368133012417
+    }, {
+      "time" : 0.8413290035988551,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.417981740760487,
+          "y" : 2.832059624899553
+        },
+        "rotation" : {
+          "radians" : -2.9647737573971904
+        }
+      },
+      "curvature" : 0.08164850595350744
+    }, {
+      "time" : 0.872270133120958,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.326667949489956,
+          "y" : 2.8153871029327426
+        },
+        "rotation" : {
+          "radians" : -2.9572307833972085
+        }
+      },
+      "curvature" : 0.08097214961252051
+    }, {
+      "time" : 0.9037887421215142,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.233781488770907,
+          "y" : 2.7976978453152483
+        },
+        "rotation" : {
+          "radians" : -2.9495838324025407
+        }
+      },
+      "curvature" : 0.08086748247871706
+    }, {
+      "time" : 0.9358164189685482,
+      "velocity" : 3.0,
+      "acceleration" : -9.119536798471233E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.1395362315633975,
+          "y" : 2.778995432319789
+        },
+        "rotation" : {
+          "radians" : -2.941795926901371
+        }
+      },
+      "curvature" : 0.08133448893648988
+    }, {
+      "time" : 0.9682807293579756,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 9.019320323200669E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.044158340126607,
+          "y" : 2.7592865290874524
+        },
+        "rotation" : {
+          "radians" : -2.9338281737918814
+        }
+      },
+      "curvature" : 0.08238157300892571
+    }, {
+      "time" : 1.0011057608042824,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9478846984741445,
+          "y" : 2.73858072970582
+        },
+        "rotation" : {
+          "radians" : -2.925639491155511
+        }
+      },
+      "curvature" : 0.08402672524453093
+    }, {
+      "time" : 1.0342126698325704,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.850961344829351,
+          "y" : 2.7168904012870905
+        },
+        "rotation" : {
+          "radians" : -2.91718624359335
+        }
+      },
+      "curvature" : 0.08629871484426285
+    }, {
+      "time" : 1.0675202322086716,
+      "velocity" : 3.0,
+      "acceleration" : -0.1315579405706849,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7536419040806095,
+          "y" : 2.694230528046205
+        },
+        "rotation" : {
+          "radians" : -2.9084217919318283
+        }
+      },
+      "curvature" : 0.08923843792521573
+    }, {
+      "time" : 1.1009699296037385,
+      "velocity" : 2.9955994266979924,
+      "acceleration" : -0.16210586852590414,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6561860202366483,
+          "y" : 2.6706185553789727
+        },
+        "rotation" : {
+          "radians" : -2.899295958219869
+        }
+      },
+      "curvature" : 0.09290053504803716
+    }, {
+      "time" : 1.1345079603171326,
+      "velocity" : 2.990162715100549,
+      "acceleration" : -0.19242213116635215,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5588577888818484,
+          "y" : 2.646074233940194
+        },
+        "rotation" : {
+          "radians" : -2.889754402955238
+        }
+      },
+      "curvature" : 0.0973553849716613
+    }, {
+      "time" : 1.1680607870231765,
+      "velocity" : 2.983706408679117,
+      "acceleration" : -0.22706531667766747,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4619241896315485,
+          "y" : 2.620619463721785
+        },
+        "rotation" : {
+          "radians" : -2.8797379082054317
+        }
+      },
+      "curvature" : 0.10269158445623167
+    }, {
+      "time" : 1.2015549415534306,
+      "velocity" : 2.976101047873854,
+      "acceleration" : -0.2670814149888477,
+      "pose" : {
+        "translation" : {
+          "x" : 3.365653518587351,
+          "y" : 2.5942781381309032
+        },
+        "rotation" : {
+          "radians" : -2.869181557635527
+        }
+      },
+      "curvature" : 0.10901903383045562
+    }, {
+      "time" : 1.2349184127471249,
+      "velocity" : 2.9671902847785026,
+      "acceleration" : -0.31371465342134314,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2703138207924285,
+          "y" : 2.567075988068072
+        },
+        "rotation" : {
+          "radians" : -2.858013802441017
+        }
+      },
+      "curvature" : 0.11647276318602535
+    }, {
+      "time" : 1.2680813779246811,
+      "velocity" : 2.9567865766514014,
+      "acceleration" : -0.3684468103258239,
+      "pose" : {
+        "translation" : {
+          "x" : 3.1761713226868302,
+          "y" : 2.539040426005304
+        },
+        "rotation" : {
+          "radians" : -2.8461554009678793
+        }
+      },
+      "curvature" : 0.12521765233260732
+    }, {
+      "time" : 1.3009769666485926,
+      "velocity" : 2.944666301912286,
+      "acceleration" : -0.43303740193218687,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0834888645627863,
+          "y" : 2.510200390064227
+        },
+        "rotation" : {
+          "radians" : -2.833518219736519
+        }
+      },
+      "curvature" : 0.1354542158098308
+    }, {
+      "time" : 1.3335420599902073,
+      "velocity" : 2.930564398497954,
+      "acceleration" : -0.5095598286786093,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9925243330200146,
+          "y" : 2.4805861880942075
+        },
+        "rotation" : {
+          "radians" : -2.820003885293717
+        }
+      },
+      "curvature" : 0.14742563679773568
+    }, {
+      "time" : 1.365718127787587,
+      "velocity" : 2.91416876690357,
+      "acceleration" : -0.6004245148597813,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9035290934210276,
+          "y" : 2.4502293417504766
+        },
+        "rotation" : {
+          "radians" : -2.8055022808143084
+        }
+      },
+      "curvature" : 0.16142623105857898
+    }, {
+      "time" : 1.3974521047697568,
+      "velocity" : 2.8951149091694792,
+      "acceleration" : -0.7083738286934076,
+      "pose" : {
+        "translation" : {
+          "x" : 2.816746422346437,
+          "y" : 2.4191624305722534
+        },
+        "rotation" : {
+          "radians" : -2.7898898902549565
+        }
+      },
+      "curvature" : 0.17781148738953634
+    }, {
+      "time" : 1.4286973034286823,
+      "velocity" : 2.87298162816717,
+      "acceleration" : -0.8769242761272262,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7324099400502604,
+          "y" : 2.38741893606087
+        },
+        "rotation" : {
+          "radians" : -2.7730280085554613
+        }
+      },
+      "curvature" : 0.1970097361358105
+    }, {
+      "time" : 1.4594210697670953,
+      "velocity" : 2.846039211610955,
+      "acceleration" : -3.9999999999999947,
+      "pose" : {
+        "translation" : {
+          "x" : 2.650742042915226,
+          "y" : 2.3550330857578956
+        },
+        "rotation" : {
+          "radians" : -2.7547608625141673
+        }
+      },
+      "curvature" : 0.2195352951517829
+    }, {
+      "time" : 1.4900955256707504,
+      "velocity" : 2.723341387996335,
+      "acceleration" : -4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5719523359080814,
+          "y" : 2.3220396973232624
+        },
+        "rotation" : {
+          "radians" : -2.7349137288234022
+        }
+      },
+      "curvature" : 0.24600255746489014
+    }, {
+      "time" : 1.5212190951177167,
+      "velocity" : 2.5988471102084696,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4962360650348963,
+          "y" : 2.2884740226133893
+        },
+        "rotation" : {
+          "radians" : -2.7132912008461836
+        }
+      },
+      "curvature" : 0.2771397985751853
+    }, {
+      "time" : 1.5528031633912385,
+      "velocity" : 2.4725108371143825,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.42377254979637,
+          "y" : 2.254371591759306
+        },
+        "rotation" : {
+          "radians" : -2.689675854298644
+        }
+      },
+      "curvature" : 0.3138003172075707
+    }, {
+      "time" : 1.5848723080728448,
+      "velocity" : 2.3442342583879574,
+      "acceleration" : -3.9999999999999942,
+      "pose" : {
+        "translation" : {
+          "x" : 2.354723615643138,
+          "y" : 2.219768057244778
+        },
+        "rotation" : {
+          "radians" : -2.6638277072718006
+        }
+      },
+      "curvature" : 0.3569666341755263
+    }, {
+      "time" : 1.6174693044153108,
+      "velocity" : 2.213846273018094,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.289232026431076,
+          "y" : 2.184699037984432
+        },
+        "rotation" : {
+          "radians" : -2.6354850773300993
+        }
+      },
+      "curvature" : 0.4077405496577045
+    }, {
+      "time" : 1.6506622716676704,
+      "velocity" : 2.0810744040086555,
+      "acceleration" : -4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2274199168766096,
+          "y" : 2.1491999634018804
+        },
+        "rotation" : {
+          "radians" : -2.6043677221311743
+        }
+      },
+      "curvature" : 0.4673075916967568
+    }, {
+      "time" : 1.6845551653471331,
+      "velocity" : 1.9455028292908045,
+      "acceleration" : -3.7890145173659073,
+      "pose" : {
+        "translation" : {
+          "x" : 2.169387225012014,
+          "y" : 2.1133059175078435
+        },
+        "rotation" : {
+          "radians" : -2.5701835156594286
+        }
+      },
+      "curvature" : 0.5368586687945079
+    }, {
+      "time" : 1.7192335134624979,
+      "velocity" : 1.8141060648434189,
+      "acceleration" : -3.2696015880426708,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1152101246407256,
+          "y" : 2.077051482978277
+        },
+        "rotation" : {
+          "radians" : -2.532640340224557
+        }
+      },
+      "curvature" : 0.6174451066520994
+    }, {
+      "time" : 1.7907419934854965,
+      "velocity" : 1.5803018250017051,
+      "acceleration" : -2.726779669291407,
+      "pose" : {
+        "translation" : {
+          "x" : 2.018599167179449,
+          "y" : 2.003596336511297
+        },
+        "rotation" : {
+          "radians" : -2.446433526852606
+        }
+      },
+      "curvature" : 0.8136612252229765
+    }, {
+      "time" : 1.8275860774684607,
+      "velocity" : 1.479836125863293,
+      "acceleration" : -2.359563803315625,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9761847286498835,
+          "y" : 1.9664608799550876
+        },
+        "rotation" : {
+          "radians" : -2.397408828393692
+        }
+      },
+      "curvature" : 0.9278899124682813
+    }, {
+      "time" : 1.8649660152071963,
+      "velocity" : 1.391635777804781,
+      "acceleration" : -1.9838468623386898,
+      "pose" : {
+        "translation" : {
+          "x" : 1.937661583645081,
+          "y" : 1.9290952336820055
+        },
+        "rotation" : {
+          "radians" : -2.3443966957035403
+        }
+      },
+      "curvature" : 1.0492344280773838
+    }, {
+      "time" : 1.902729666191217,
+      "velocity" : 1.3167184772896783,
+      "acceleration" : -1.5953761316272526,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9029635716538653,
+          "y" : 1.891529134866047
+        },
+        "rotation" : {
+          "radians" : -2.2876080290002703
+        }
+      },
+      "curvature" : 1.1720276125742721
+    }, {
+      "time" : 1.940679658288589,
+      "velocity" : 1.256173965702088,
+      "acceleration" : -1.186040735740344,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8719913626680516,
+          "y" : 1.85379088381519
+        },
+        "rotation" : {
+          "radians" : -2.227526882083615
+        }
+      },
+      "curvature" : 1.2877279640741648
+    }, {
+      "time" : 1.9785675742743187,
+      "velocity" : 1.2112373539507049,
+      "acceleration" : -0.7410587042667358,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8446108896377584,
+          "y" : 1.8159071880495183
+        },
+        "rotation" : {
+          "radians" : -2.164970018044204
+        }
+      },
+      "curvature" : 1.3850491721536529
+    }, {
+      "time" : 2.0160893592543716,
+      "velocity" : 1.1834315085916118,
+      "acceleration" : -0.23332707042705494,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8206517809267098,
+          "y" : 1.777903006379347
+        },
+        "rotation" : {
+          "radians" : -2.1011212361539733
+        }
+      },
+      "curvature" : 1.4508998888713995
+    }, {
+      "time" : 2.052881886984702,
+      "velocity" : 1.1748468158826877,
+      "acceleration" : 0.3888458638588137,
+      "pose" : {
+        "translation" : {
+          "x" : 1.799905792767543,
+          "y" : 1.7398013929833471
+        },
+        "rotation" : {
+          "radians" : -2.037523308037849
+        }
+      },
+      "curvature" : 1.4721810231732262
+    }, {
+      "time" : 2.088519327343306,
+      "velocity" : 1.188704287164646,
+      "acceleration" : 1.231210997720917,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7821252417171145,
+          "y" : 1.7016233414866697
+        },
+        "rotation" : {
+          "radians" : -1.976018179939325
+        }
+      },
+      "curvature" : 1.4380568182608686
+    }, {
+      "time" : 2.1225056544060728,
+      "velocity" : 1.2305486268164643,
+      "acceleration" : 2.534651579871874,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7670214371118043,
+          "y" : 1.6633876290390703
+        },
+        "rotation" : {
+          "radians" : -1.918641555195968
+        }
+      },
+      "curvature" : 1.341918509745125
+    }, {
+      "time" : 2.1542554976740123,
+      "velocity" : 1.311023417216232,
+      "acceleration" : 3.9999999999999845,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7542631135228248,
+          "y" : 1.6251106603930339
+        },
+        "rotation" : {
+          "radians" : -1.8674949603224105
+        }
+      },
+      "curvature" : 1.182232258965127
+    }, {
+      "time" : 2.2099359231623428,
+      "velocity" : 1.5337451191695526,
+      "acceleration" : 4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7342355685846904,
+          "y" : 1.5484857759979853
+        },
+        "rotation" : {
+          "radians" : -1.7919535583318533
+        }
+      },
+      "curvature" : 0.6868485079079413
+    }, {
+      "time" : 2.2579553489086845,
+      "velocity" : 1.7258228221549206,
+      "acceleration" : -1.2499750022631253,
+      "pose" : {
+        "translation" : {
+          "x" : 1.718481421470642,
+          "y" : 1.4718265533447266
+        },
+        "rotation" : {
+          "radians" : -1.7639859651971548
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 2.3039131087144136,
+      "velocity" : 1.6683767712377462,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7040171629614953,
+          "y" : 1.3951845909212786
+        },
+        "rotation" : {
+          "radians" : -1.744243059159777
+        }
+      },
+      "curvature" : 0.4949885871870906
+    }, {
+      "time" : 2.353201866781118,
+      "velocity" : 1.4712217389709286,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 1.692568655125001,
+          "y" : 1.3186628076501847
+        },
+        "rotation" : {
+          "radians" : -1.688734274061912
+        }
+      },
+      "curvature" : 0.9318135483719395
+    }, {
+      "time" : 2.409519830422982,
+      "velocity" : 1.2459498844034724,
+      "acceleration" : -2.1690899330402935,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6866636401594342,
+          "y" : 1.2423782268174186
+        },
+        "rotation" : {
+          "radians" : -1.6025452180114088
+        }
+      },
+      "curvature" : 1.3089484866875387
+    }, {
+      "time" : 2.4408900706331065,
+      "velocity" : 1.1779050121666355,
+      "acceleration" : -1.4371594032530797,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6864417690519602,
+          "y" : 1.204360419059464
+        },
+        "rotation" : {
+          "radians" : -1.5497323811772583
+        }
+      },
+      "curvature" : 1.4645464958895065
+    }, {
+      "time" : 2.4737794652080054,
+      "velocity" : 1.130637709486019,
+      "acceleration" : -0.882454393290906,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6883282148664875,
+          "y" : 1.1664440317872504
+        },
+        "rotation" : {
+          "radians" : -1.4916448185226532
+        }
+      },
+      "curvature" : 1.589559435549557
+    }, {
+      "time" : 2.5078700922418036,
+      "velocity" : 1.100554285890002,
+      "acceleration" : -0.4285512346859183,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6925034029607406,
+          "y" : 1.1286425412588699
+        },
+        "rotation" : {
+          "radians" : -1.4293799219170706
+        }
+      },
+      "curvature" : 1.677647696189635
+    }, {
+      "time" : 2.542863859363714,
+      "velocity" : 1.0855576637835957,
+      "acceleration" : -0.03409957451987376,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6991206308842841,
+          "y" : 1.0909691291280303
+        },
+        "rotation" : {
+          "radians" : -1.3641679929709554
+        }
+      },
+      "curvature" : 1.7243201694504038
+    }, {
+      "time" : 2.578478719165117,
+      "velocity" : 1.084343212217783,
+      "acceleration" : 0.32480160779729494,
+      "pose" : {
+        "translation" : {
+          "x" : 1.708307124635804,
+          "y" : 1.0534366687917367
+        },
+        "rotation" : {
+          "radians" : -1.2973160315953416
+        }
+      },
+      "curvature" : 1.72818476876106
+    }, {
+      "time" : 2.6144494673900835,
+      "velocity" : 1.0960265690749238,
+      "acceleration" : 0.662753960787673,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7201650949203895,
+          "y" : 1.0160577117379717
+        },
+        "rotation" : {
+          "radians" : -1.2301287631472986
+        }
+      },
+      "curvature" : 1.691537146255751
+    }, {
+      "time" : 2.6505308994153998,
+      "velocity" : 1.1199396810605933,
+      "acceleration" : 0.9891536088281452,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7347727934068158,
+          "y" : 0.9788444738933771
+        },
+        "rotation" : {
+          "radians" : -1.163822987935704
+        }
+      },
+      "curvature" : 1.6200724563638298
+    }, {
+      "time" : 2.686501528745972,
+      "velocity" : 1.1555201588747483,
+      "acceleration" : 1.310397118703277,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7521855689848245,
+          "y" : 0.941808821970934
+        },
+        "rotation" : {
+          "radians" : -1.0994531238813727
+        }
+      },
+      "curvature" : 1.52183879385189
+    }, {
+      "time" : 2.7221665450113437,
+      "velocity" : 1.202255493427397,
+      "acceleration" : 1.6311499809238765,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7724369240224052,
+          "y" : 0.9049622598176431
+        },
+        "rotation" : {
+          "radians" : -1.0378615567095117
+        }
+      },
+      "curvature" : 1.405821442899369
+    }, {
+      "time" : 2.757359258068479,
+      "velocity" : 1.2596600866592031,
+      "acceleration" : 1.9551274988150928,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7955395706230775,
+          "y" : 0.8683159147622064
+        },
+        "rotation" : {
+          "radians" : -0.9796592961829077
+        }
+      },
+      "curvature" : 1.2806102287854289
+    }, {
+      "time" : 2.7919408310448324,
+      "velocity" : 1.3272714709375524,
+      "acceleration" : 2.2856166223411245,
+      "pose" : {
+        "translation" : {
+          "x" : 1.821486486883173,
+          "y" : 0.8318805239627071
+        },
+        "rotation" : {
+          "radians" : -0.9252342480030827
+        }
+      },
+      "curvature" : 1.1534643716591353
+    }, {
+      "time" : 2.825798523286439,
+      "velocity" : 1.4046571751190786,
+      "acceleration" : 2.6258637992019844,
+      "pose" : {
+        "translation" : {
+          "x" : 1.850251973149117,
+          "y" : 0.7956664207542907
+        },
+        "rotation" : {
+          "radians" : -0.8747791350162617
+        }
+      },
+      "curvature" : 1.0298714520728465
+    }, {
+      "time" : 2.8588428851625496,
+      "velocity" : 1.4914271687372875,
+      "acceleration" : 2.9794041928936728,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8817927082747086,
+          "y" : 0.7596835209968459
+        },
+        "rotation" : {
+          "radians" : -0.8283296056482383
+        }
+      },
+      "curvature" : 0.9135232462092671
+    }, {
+      "time" : 2.891004389345329,
+      "velocity" : 1.5872492891492276,
+      "acceleration" : 3.5509647371659865,
+      "pose" : {
+        "translation" : {
+          "x" : 1.916048805878404,
+          "y" : 0.7239413094226848
+        },
+        "rotation" : {
+          "radians" : -0.7858044305587907
+        }
+      },
+      "curvature" : 0.8065539475387932
+    }, {
+      "time" : 2.9523592966906644,
+      "velocity" : 1.8051184015845994,
+      "acceleration" : 3.999999999999984,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9923910543609047,
+          "y" : 0.6532146522016653
+        },
+        "rotation" : {
+          "radians" : -0.7118327324730775
+        }
+      },
+      "curvature" : 0.6236089171423088
+    }, {
+      "time" : 3.0100353656283176,
+      "velocity" : 2.035822677335212,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0785084606141027,
+          "y" : 0.5835531856100715
+        },
+        "rotation" : {
+          "radians" : -0.6511181043270622
+        }
+      },
+      "curvature" : 0.4800010235812804
+    }, {
+      "time" : 3.0646190962309157,
+      "velocity" : 2.2541575997456036,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1734333233560115,
+          "y" : 0.5150158743770881
+        },
+        "rotation" : {
+          "radians" : -0.6017399678104041
+        }
+      },
+      "curvature" : 0.3691740752256128
+    }, {
+      "time" : 3.1166653761479624,
+      "velocity" : 2.46234271941379,
+      "acceleration" : 4.000000000000028,
+      "pose" : {
+        "translation" : {
+          "x" : 2.276034298238187,
+          "y" : 0.4476534745680283
+        },
+        "rotation" : {
+          "radians" : -0.5619346801385068
+        }
+      },
+      "curvature" : 0.2836399510576229
+    }, {
+      "time" : 3.1418275733370513,
+      "velocity" : 2.562991508170146,
+      "acceleration" : 4.0000000000000275,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3298233544980946,
+          "y" : 0.4144263756493385
+        },
+        "rotation" : {
+          "radians" : -0.5451464563946322
+        }
+      },
+      "curvature" : 0.24824311811855138
+    }, {
+      "time" : 3.1664400864835103,
+      "velocity" : 2.6614415607559834,
+      "acceleration" : 3.999999999999966,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3850501980787406,
+          "y" : 0.38150809671011743
+        },
+        "rotation" : {
+          "radians" : -0.5302189330250398
+        }
+      },
+      "curvature" : 0.21679631372622093
+    }, {
+      "time" : 3.1905125871491076,
+      "velocity" : 2.757731563418372,
+      "acceleration" : 4.000000000000013,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4415424379903916,
+          "y" : 0.3489024790262656
+        },
+        "rotation" : {
+          "radians" : -0.5170134828220253
+        }
+      },
+      "curvature" : 0.1886809876230122
+    }, {
+      "time" : 3.214049672205038,
+      "velocity" : 2.851879903642094,
+      "acceleration" : 3.303657668196632,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4991237930953503,
+          "y" : 0.31661276891827583
+        },
+        "rotation" : {
+          "radians" : -0.5054087566305369
+        }
+      },
+      "curvature" : 0.16335410873820064
+    }, {
+      "time" : 3.2371151553370474,
+      "velocity" : 2.928080363861817,
+      "acceleration" : 1.0968430910701312,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5576151483652376,
+          "y" : 0.2846416040989146
+        },
+        "rotation" : {
+          "radians" : -0.49529950641493536
+        }
+      },
+      "curvature" : 0.14033935814094875
+    }, {
+      "time" : 3.259949848690555,
+      "velocity" : 2.953126439503317,
+      "acceleration" : 1.0318522973125155,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6168356111382742,
+          "y" : 0.25299100002090213
+        },
+        "rotation" : {
+          "radians" : -0.48659537038691486
+        }
+      },
+      "curvature" : 0.11921786782590763
+    }, {
+      "time" : 3.2827100656165435,
+      "velocity" : 2.9766116216257297,
+      "acceleration" : 0.9835246105005508,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6766035673765614,
+          "y" : 0.22166233622459286
+        },
+        "rotation" : {
+          "radians" : -0.47921968759438144
+        }
+      },
+      "curvature" : 0.09961923590719055
+    }, {
+      "time" : 3.305354934644453,
+      "velocity" : 2.99888340761624,
+      "acceleration" : 0.04948814588093301,
+      "pose" : {
+        "translation" : {
+          "x" : 2.736737737923363,
+          "y" : 0.19065634268565645
+        },
+        "rotation" : {
+          "radians" : -0.4731083784610581
+        }
+      },
+      "curvature" : 0.08121320610983931
+    }, {
+      "time" : 3.3279177598197176,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.3150721678562816E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7970582347603883,
+          "y" : 0.15997308616275907
+        },
+        "rotation" : {
+          "radians" : -0.4682089061577221
+        }
+      },
+      "curvature" : 0.063702197976732
+    }, {
+      "time" : 3.3504305526891756,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.857387617265072,
+          "y" : 0.1296119565452436
+        },
+        "rotation" : {
+          "radians" : -0.4644793182582315
+        }
+      },
+      "curvature" : 0.0468147679013892
+    }, {
+      "time" : 3.3728462381053794,
+      "velocity" : 3.0,
+      "acceleration" : -1.3294976180711803E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9175519484678567,
+          "y" : 0.09957165320080996
+        },
+        "rotation" : {
+          "radians" : -0.461887356526069
+        }
+      },
+      "curvature" : 0.03030003960646033
+    }, {
+      "time" : 3.395114760380951,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.3412784420541495E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 2.977381851309474,
+          "y" : 0.06985017132319626
+        },
+        "rotation" : {
+          "radians" : -0.4604096131005984
+        }
+      },
+      "curvature" : 0.013923147393524328
+    }, {
+      "time" : 3.4171876919923743,
+      "velocity" : 3.0,
+      "acceleration" : -0.04926038986669166,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0367135648982266,
+          "y" : 0.04044478827985998
+        },
+        "rotation" : {
+          "radians" : -0.46003070242576016
+        }
+      },
+      "curvature" : -0.0025382237303508883
+    }, {
+      "time" : 3.43902255151928,
+      "velocity" : 2.99892440630702,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0953900007672708,
+          "y" : 0.011352049959658217
+        },
+        "rotation" : {
+          "radians" : -0.4607424088911207
+        }
+      },
+      "curvature" : -0.019296044170462233
+    }, {
+      "time" : 3.460894301999556,
+      "velocity" : 2.911437404385916,
+      "acceleration" : -4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 3.153261799131896,
+          "y" : -0.017432242879471982
+        },
+        "rotation" : {
+          "radians" : -0.46254275944377354
+        }
+      },
+      "curvature" : -0.03655263042494752
+    }, {
+      "time" : 3.483096236841796,
+      "velocity" : 2.8226296650169553,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2101883851468074,
+          "y" : -0.04591304826283249
+        },
+        "rotation" : {
+          "radians" : -0.4654349576602489
+        }
+      },
+      "curvature" : -0.05449941944261404
+    }, {
+      "time" : 3.5284559791470897,
+      "velocity" : 2.6411906957957814,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 3.320693882987083,
+          "y" : -0.10198790459356033
+        },
+        "rotation" : {
+          "radians" : -0.4745255802104961
+        }
+      },
+      "curvature" : -0.09314935868314721
+    }, {
+      "time" : 3.5750713755207792,
+      "velocity" : 2.454729110301023,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4259977320907637,
+          "y" : -0.1569278776878491
+        },
+        "rotation" : {
+          "radians" : -0.488085849209704
+        }
+      },
+      "curvature" : -0.13632788196047352
+    }, {
+      "time" : 3.6230020187291068,
+      "velocity" : 2.2630065374677133,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 3.525399329757196,
+          "y" : -0.21080134048127874
+        },
+        "rotation" : {
+          "radians" : -0.5061441065978821
+        }
+      },
+      "curvature" : -0.18426344503119224
+    }, {
+      "time" : 3.6724555712310893,
+      "velocity" : 2.065192327459783,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6184400330154176,
+          "y" : -0.2636901170639021
+        },
+        "rotation" : {
+          "radians" : -0.528561557547075
+        }
+      },
+      "curvature" : -0.2354062697850754
+    }, {
+      "time" : 3.723886359469485,
+      "velocity" : 1.8594691745062002,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7049369588571714,
+          "y" : -0.3156899195544618
+        },
+        "rotation" : {
+          "radians" : -0.5548405336431187
+        }
+      },
+      "curvature" : -0.28492058176951146
+    }, {
+      "time" : 3.7781787791469426,
+      "velocity" : 1.6422994957963697,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 3.785016784469917,
+          "y" : -0.36691078497460694
+        },
+        "rotation" : {
+          "radians" : -0.5838549927785415
+        }
+      },
+      "curvature" : -0.32275632767074536
+    }, {
+      "time" : 3.837038685288918,
+      "velocity" : 1.4068598712284692,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.859149547469844,
+          "y" : -0.41747751212311
+        },
+        "rotation" : {
+          "radians" : -0.6135313524400181
+        }
+      },
+      "curvature" : -0.3322823649480369
+    }, {
+      "time" : 3.9040276125177047,
+      "velocity" : 1.1389041623133211,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9281824461348833,
+          "y" : -0.46753009845008364
+        },
+        "rotation" : {
+          "radians" : -0.6405875389886441
+        }
+      },
+      "curvature" : -0.29180251710931904
+    }, {
+      "time" : 3.988546345848521,
+      "velocity" : 0.800829228990057,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9933736396377206,
+          "y" : -0.5172241769311974
+        },
+        "rotation" : {
+          "radians" : -0.6605456517130821
+        }
+      },
+      "curvature" : -0.1820147647738938
+    }, {
+      "time" : 4.188753653096035,
+      "velocity" : 0.0,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 4.056426048278809,
+          "y" : -0.5667314529418945
+        },
+        "rotation" : {
+          "radians" : -0.6682894149758353
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.807385444641113, -3.022507667541504, 0.0 ],
+      "y" : [ 2.9820399284362793, -0.10218214988708496, 0.0 ]
+    }, {
+      "x" : [ 1.718481421470642, -0.47991621494293213, 0.0 ],
+      "y" : [ 1.4718265533447266, -2.453189492225647, 0.0 ]
+    }, {
+      "x" : [ 4.056426048278809, 2.0053954124450684, 0.0 ],
+      "y" : [ -0.5667314529418945, -1.5832068920135498, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : -3.127725471555593
+      }
+    }, {
+      "time" : 2.249999949708581,
+      "rotation" : {
+        "radians" : -0.798910986801039
+      }
+    }, {
+      "time" : 4.099999908357859,
+      "rotation" : {
+        "radians" : -0.6659689899224528
+      }
+    } ],
+    "reversed" : false,
+    "color" : 192.47057,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@t 2\nIntake.setWantedIntakeState EJECT",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 2.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "EJECT" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "@t 2.7\nDrive.setAutoRotation 35",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 2.7,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Drive.setAutoRotation",
+        "args" : [ "35" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.056426048278809,
+          "y" : -0.5667314529418945
+        },
+        "rotation" : {
+          "radians" : 2.4520537478581654
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.18493870993194386,
+      "velocity" : 0.7397548397277754,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 4.003633991609843,
+          "y" : -0.5232320818440126
+        },
+        "rotation" : {
+          "radians" : 2.4530901867155106
+        }
+      },
+      "curvature" : 0.029752937013657187
+    }, {
+      "time" : 0.26148282578134485,
+      "velocity" : 1.0459313031253796,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9508028739061523,
+          "y" : -0.47987904225311695
+        },
+        "rotation" : {
+          "radians" : 2.4560494170073883
+        }
+      },
+      "curvature" : 0.05635458173870567
+    }, {
+      "time" : 0.32012947576533146,
+      "velocity" : 1.2805179030613263,
+      "acceleration" : 3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 3.897901180103951,
+          "y" : -0.4368061550801574
+        },
+        "rotation" : {
+          "radians" : 2.4607189045800832
+        }
+      },
+      "curvature" : 0.08009487969635327
+    }, {
+      "time" : 0.36946312936714043,
+      "velocity" : 1.477852517468562,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8449053191515077,
+          "y" : -0.39413398946635425
+        },
+        "rotation" : {
+          "radians" : 2.466900851787993
+        }
+      },
+      "curvature" : 0.10120927083782304
+    }, {
+      "time" : 0.4128042913826585,
+      "velocity" : 1.6512171655306345,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 3.791799285257279,
+          "y" : -0.35197029855173945
+        },
+        "rotation" : {
+          "radians" : 2.474409887161352
+        }
+      },
+      "curvature" : 0.1198860351486836
+    }, {
+      "time" : 0.451853631575856,
+      "velocity" : 1.8074145263034247,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.738574319138042,
+          "y" : -0.3104104552436979
+        },
+        "rotation" : {
+          "radians" : 2.483071079673296
+        }
+      },
+      "curvature" : 0.13627342891926963
+    }, {
+      "time" : 0.4876202915697968,
+      "velocity" : 1.9504811662791879,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6852285692670304,
+          "y" : -0.2695378879855088
+        },
+        "rotation" : {
+          "radians" : 2.4927182713379636
+        }
+      },
+      "curvature" : 0.15048631581735405
+    }, {
+      "time" : 0.5207612575253416,
+      "velocity" : 2.0830450301013674,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.631766753122065,
+          "y" : -0.2294245165248867
+        },
+        "rotation" : {
+          "radians" : 2.5031927075423317
+        }
+      },
+      "curvature" : 0.16261213661992346
+    }, {
+      "time" : 0.5517326643887606,
+      "velocity" : 2.206930657555043,
+      "acceleration" : 3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5781998184336885,
+          "y" : -0.1901311876825229
+        },
+        "rotation" : {
+          "radians" : 2.514341935981025
+        }
+      },
+      "curvature" : 0.1727161388917002
+    }, {
+      "time" : 0.5808665583284028,
+      "velocity" : 2.3234662333136122,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5245446044332986,
+          "y" : -0.15170811112062665
+        },
+        "rotation" : {
+          "radians" : 2.526018940159826
+        }
+      },
+      "curvature" : 0.18084582936508686
+    }, {
+      "time" : 0.6084135883193114,
+      "velocity" : 2.4336543532772463,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.470823503101282,
+          "y" : -0.11419529511146642
+        },
+        "rotation" : {
+          "radians" : 2.538081470866874
+        }
+      },
+      "curvature" : 0.18703463399877743
+    }, {
+      "time" : 0.6345684464707756,
+      "velocity" : 2.5382737858831033,
+      "acceleration" : 3.9999999999999862,
+      "pose" : {
+        "translation" : {
+          "x" : 3.417064120415148,
+          "y" : -0.07762298230591114
+        },
+        "rotation" : {
+          "radians" : 2.5503915380457647
+        }
+      },
+      "curvature" : 0.19130476386228212
+    }, {
+      "time" : 0.6594858696162618,
+      "velocity" : 2.6379434784650475,
+      "acceleration" : 3.999999999999993,
+      "pose" : {
+        "translation" : {
+          "x" : 3.36329893759766,
+          "y" : -0.04201208550197144
+        },
+        "rotation" : {
+          "radians" : 2.5628150257457696
+        }
+      },
+      "curvature" : 0.19366929489891171
+    }, {
+      "time" : 0.6832911528298411,
+      "velocity" : 2.733164611319365,
+      "acceleration" : 3.0356954614896483,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3095649723649707,
+          "y" : -0.007374623413340942
+        },
+        "rotation" : {
+          "radians" : 2.575221394062975
+        }
+      },
+      "curvature" : 0.1941334791085683
+    }, {
+      "time" : 0.7282945981321051,
+      "velocity" : 2.8697813659748452,
+      "acceleration" : 0.35602362885687006,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2023594154743478,
+          "y" : 0.05897377757355571
+        },
+        "rotation" : {
+          "radians" : 2.5994770448888582
+        }
+      },
+      "curvature" : 0.18934541322883708
+    }, {
+      "time" : 0.7712234425204774,
+      "velocity" : 2.8850650489366254,
+      "acceleration" : 0.5853616532790817,
+      "pose" : {
+        "translation" : {
+          "x" : 3.095821448769353,
+          "y" : 0.1214873934270031
+        },
+        "rotation" : {
+          "radians" : 2.622176713767235
+        }
+      },
+      "curvature" : 0.17683058489309955
+    }, {
+      "time" : 0.8129042620605731,
+      "velocity" : 2.909463402372643,
+      "acceleration" : 0.8532903307335525,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9903759750508243,
+          "y" : 0.18034581308165798
+        },
+        "rotation" : {
+          "radians" : 2.6423801962411266
+        }
+      },
+      "curvature" : 0.156321766875571
+    }, {
+      "time" : 0.8531469929208557,
+      "velocity" : 2.943802135498035,
+      "acceleration" : 0.5959644267737211,
+      "pose" : {
+        "translation" : {
+          "x" : 2.886487960834632,
+          "y" : 0.23582909704128951
+        },
+        "rotation" : {
+          "radians" : 2.659177929985617
+        }
+      },
+      "curvature" : 0.12732676169969878
+    }, {
+      "time" : 0.8919109189015986,
+      "velocity" : 2.966904056424647,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 2.784651596291951,
+          "y" : 0.28830383278545924
+        },
+        "rotation" : {
+          "radians" : 2.6716758173179893
+        }
+      },
+      "curvature" : 0.0890361198334152
+    }, {
+      "time" : 0.9303572438648844,
+      "velocity" : 2.813118756571504,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.68537945518953,
+          "y" : 0.338209190176201
+        },
+        "rotation" : {
+          "radians" : 2.6789757213651453
+        }
+      },
+      "curvature" : 0.04021801225466915
+    }, {
+      "time" : 0.9696416444039297,
+      "velocity" : 2.6559811544153225,
+      "acceleration" : -4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5891916548299605,
+          "y" : 0.3860429768647009
+        },
+        "rotation" : {
+          "radians" : 2.680153087048729
+        }
+      },
+      "curvature" : -0.02088202984869535
+    }, {
+      "time" : 1.0098343280634516,
+      "velocity" : 2.4952104197772345,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4966050159919497,
+          "y" : 0.4323476936979773
+        },
+        "rotation" : {
+          "radians" : 2.6742351029390625
+        }
+      },
+      "curvature" : -0.09660379275555367
+    }, {
+      "time" : 1.0510425689247183,
+      "velocity" : 2.3303774563321675,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4081222228705883,
+          "y" : 0.47769659012556076
+        },
+        "rotation" : {
+          "radians" : 2.660185719014542
+        }
+      },
+      "curvature" : -0.18983275500917693
+    }, {
+      "time" : 1.0934364510155294,
+      "velocity" : 2.160801927968923,
+      "acceleration" : -4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3242209830176215,
+          "y" : 0.5226797196061739
+        },
+        "rotation" : {
+          "radians" : 2.636908345303076
+        }
+      },
+      "curvature" : -0.30373535138801905
+    }, {
+      "time" : 1.1372916096013064,
+      "velocity" : 1.9853812936258146,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 2.245343187281719,
+          "y" : 0.5678899950144114
+        },
+        "rotation" : {
+          "radians" : 2.6032836790059384
+        }
+      },
+      "curvature" : -0.4410606681722093
+    }, {
+      "time" : 1.1830628813303603,
+      "velocity" : 1.8022962067095987,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.171884069748746,
+          "y" : 0.6139092440474201
+        },
+        "rotation" : {
+          "radians" : 2.5582681034202466
+        }
+      },
+      "curvature" : -0.6027205654193771
+    }, {
+      "time" : 1.2315199026124195,
+      "velocity" : 1.6084681215813614,
+      "acceleration" : -3.2340381044624498,
+      "pose" : {
+        "translation" : {
+          "x" : 2.104181367682031,
+          "y" : 0.6612942646315787
+        },
+        "rotation" : {
+          "radians" : 2.501083096666535
+        }
+      },
+      "curvature" : -0.7854142700643647
+    }, {
+      "time" : 1.2832918933759534,
+      "velocity" : 1.441035530708215,
+      "acceleration" : -2.161144532997304,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0425044814626396,
+          "y" : 0.7105628803291779
+        },
+        "rotation" : {
+          "radians" : 2.431516535185916
+        }
+      },
+      "curvature" : -0.978530406846595
+    }, {
+      "time" : 1.3381225717901073,
+      "velocity" : 1.322538509812933,
+      "acceleration" : -1.509248244590621,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9870436345296412,
+          "y" : 0.7621799957451003
+        },
+        "rotation" : {
+          "radians" : 2.3503134945481206
+        }
+      },
+      "curvature" : -1.161734937054165
+    }, {
+      "time" : 1.3664915633080867,
+      "velocity" : 1.2797226591636164,
+      "acceleration" : -1.1263537161568526,
+      "pose" : {
+        "translation" : {
+          "x" : 1.961679353004223,
+          "y" : 0.7889966218317799
+        },
+        "rotation" : {
+          "radians" : 2.3059373339142457
+        }
+      },
+      "curvature" : -1.24077199916201
+    }, {
+      "time" : 1.3952936681311403,
+      "velocity" : 1.2472813013630308,
+      "acceleration" : -0.7667596602916469,
+      "pose" : {
+        "translation" : {
+          "x" : 1.937899033320381,
+          "y" : 0.8165436519335003
+        },
+        "rotation" : {
+          "radians" : 2.2595451461663565
+        }
+      },
+      "curvature" : -1.306155490343675
+    }, {
+      "time" : 1.424400522054752,
+      "velocity" : 1.2249633399364037,
+      "acceleration" : -0.42132507119357915,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9156993576789647,
+          "y" : 0.8448581059673881
+        },
+        "rotation" : {
+          "radians" : 2.2116259567064747
+        }
+      },
+      "curvature" : -1.3541835082656821
+    }, {
+      "time" : 1.4536761435963022,
+      "velocity" : 1.2126287866061738,
+      "acceleration" : -0.08170141033781976,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8950700272107497,
+          "y" : 0.8739710818044841
+        },
+        "rotation" : {
+          "radians" : 2.162745065781283
+        }
+      },
+      "curvature" : -1.3818724424188715
+    }, {
+      "time" : 1.4829785112196618,
+      "velocity" : 1.210234741845108,
+      "acceleration" : 0.26045790812859687,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8759934232245703,
+          "y" : 0.9039073195012026
+        },
+        "rotation" : {
+          "radians" : 2.113522302547328
+        }
+      },
+      "curvature" : -1.387344994997751
+    }, {
+      "time" : 1.5121614711143974,
+      "velocity" : 1.2178356745322916,
+      "acceleration" : 0.6143648008907316,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8584442684554539,
+          "y" : 0.9346847655307897
+        },
+        "rotation" : {
+          "radians" : 2.064604229359281
+        }
+      },
+      "curvature" : -1.3700812404747664
+    }, {
+      "time" : 1.5410766974782304,
+      "velocity" : 1.2356001718200182,
+      "acceleration" : 0.9912142506014653,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8423892883127544,
+          "y" : 0.9663141370147823
+        },
+        "rotation" : {
+          "radians" : 2.0166336726291316
+        }
+      },
+      "curvature" : -1.3309685196438428
+    }, {
+      "time" : 1.5695753786663202,
+      "velocity" : 1.2638484707370008,
+      "acceleration" : 1.6439611484263257,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8277868721282857,
+          "y" : 0.9987984859544667
+        },
+        "rotation" : {
+          "radians" : 1.9702206136785245
+        }
+      },
+      "curvature" : -1.2721364283154866
+    }, {
+      "time" : 1.6245807356379516,
+      "velocity" : 1.3542751405536837,
+      "acceleration" : 2.798992607878158,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8027295760623936,
+          "y" : 1.0663033839935565
+        },
+        "rotation" : {
+          "radians" : 1.8842065669188885
+        }
+      },
+      "curvature" : -1.1079237968679712
+    }, {
+      "time" : 1.676119381405686,
+      "velocity" : 1.4985314290776237,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 1.782759900790552,
+          "y" : 1.137054014048772
+        },
+        "rotation" : {
+          "radians" : 1.8100759100562795
+        }
+      },
+      "curvature" : -0.904882088448944
+    }, {
+      "time" : 1.7234002817652252,
+      "velocity" : 1.6876550305157805,
+      "acceleration" : 3.9999999999999885,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7672103094854315,
+          "y" : 1.2107543994101775
+        },
+        "rotation" : {
+          "radians" : 1.7501344274699886
+        }
+      },
+      "curvature" : -0.6861541138888728
+    }, {
+      "time" : 1.7668603468362696,
+      "velocity" : 1.8614952907999578,
+      "acceleration" : -2.6118583184812723,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7552473678998695,
+          "y" : 1.2869440876638691
+        },
+        "rotation" : {
+          "radians" : 1.7057998025006624
+        }
+      },
+      "curvature" : -0.4641078115451259
+    }, {
+      "time" : 1.8104168065342312,
+      "velocity" : 1.7477319892142427,
+      "acceleration" : -4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7458609043071398,
+          "y" : 1.3649842060986543
+        },
+        "rotation" : {
+          "radians" : 1.6781427516970726
+        }
+      },
+      "curvature" : -0.23909570910678915
+    }, {
+      "time" : 1.858532964177138,
+      "velocity" : 1.5552673586426153,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7378531694412231,
+          "y" : 1.444043517112732
+        },
+        "rotation" : {
+          "radians" : 1.668497763241333
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 1.9135617600953347,
+      "velocity" : 1.3351521749698287,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 1.731150510518038,
+          "y" : 1.5232887159155766
+        },
+        "rotation" : {
+          "radians" : 1.6290693879713087
+        }
+      },
+      "curvature" : -0.9651764780400796
+    }, {
+      "time" : 1.9447141490181088,
+      "velocity" : 1.2105426192787325,
+      "acceleration" : -3.8758391033227886,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7297123504678038,
+          "y" : 1.5629148639371933
+        },
+        "rotation" : {
+          "radians" : 1.582290663183503
+        }
+      },
+      "curvature" : -1.3866393975350608
+    }, {
+      "time" : 1.9793752955290544,
+      "velocity" : 1.0762015922656092,
+      "acceleration" : -2.0744901253079546,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7304462942776127,
+          "y" : 1.6025386552453256
+        },
+        "rotation" : {
+          "radians" : 1.5198280388303091
+        }
+      },
+      "curvature" : -1.7544316082551803
+    }, {
+      "time" : 2.0177456093542387,
+      "velocity" : 0.9966027551302967,
+      "acceleration" : -1.0463551078296154,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7339390283538525,
+          "y" : 1.6421520518968291
+        },
+        "rotation" : {
+          "radians" : 1.4439430669964353
+        }
+      },
+      "curvature" : -2.0458770787946707
+    }, {
+      "time" : 2.0589400093906947,
+      "velocity" : 0.9534987842381746,
+      "acceleration" : -0.4880905611008773,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7407113486278938,
+          "y" : 1.6817436414219813
+        },
+        "rotation" : {
+          "radians" : 1.3575512036532698
+        }
+      },
+      "curvature" : -2.2350303059988965
+    }, {
+      "time" : 2.0803982879163243,
+      "velocity" : 0.9430252010323411,
+      "acceleration" : -0.17603090665285,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7454726503711389,
+          "y" : 1.70152681499276
+        },
+        "rotation" : {
+          "radians" : 1.3115071881305223
+        }
+      },
+      "curvature" : -2.2849521300818747
+    }, {
+      "time" : 2.1022775055735607,
+      "velocity" : 0.9391737825112827,
+      "acceleration" : 0.10804204147343682,
+      "pose" : {
+        "translation" : {
+          "x" : 1.751220456348885,
+          "y" : 1.7212988221176222
+        },
+        "rotation" : {
+          "radians" : 1.2642073316351676
+        }
+      },
+      "curvature" : -2.3037310856346584
+    }, {
+      "time" : 2.124492132931374,
+      "velocity" : 0.9415738962015925,
+      "acceleration" : 0.3726592944849579,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7580023567016838,
+          "y" : 1.7410573696656044
+        },
+        "rotation" : {
+          "radians" : 1.2161486950907356
+        }
+      },
+      "curvature" : -2.2920014286625774
+    }, {
+      "time" : 2.146960488972026,
+      "velocity" : 0.9499469379119386,
+      "acceleration" : 0.6237338591278402,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7658622538765485,
+          "y" : 1.7607999883402954
+        },
+        "rotation" : {
+          "radians" : 1.1678204422268519
+        }
+      },
+      "curvature" : -2.2517750837082873
+    }, {
+      "time" : 2.1696053252037935,
+      "velocity" : 0.9640712890040968,
+      "acceleration" : 0.8654335988347971,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7748404343704771,
+          "y" : 1.780524038470248
+        },
+        "rotation" : {
+          "radians" : 1.119686904181982
+        }
+      },
+      "curvature" : -2.1862781024111237
+    }, {
+      "time" : 2.192354484026944,
+      "velocity" : 0.9837591753948806,
+      "acceleration" : 1.1007322784603935,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7849736404739778,
+          "y" : 1.8002267157993888
+        },
+        "rotation" : {
+          "radians" : 1.0721723951266355
+        }
+      },
+      "curvature" : -2.0996461580977543
+    }, {
+      "time" : 2.215141507868605,
+      "velocity" : 1.008841588067443,
+      "acceleration" : 1.3317674781680595,
+      "pose" : {
+        "translation" : {
+          "x" : 1.796295142014595,
+          "y" : 1.819905057277429
+        },
+        "rotation" : {
+          "radians" : 1.025649276755208
+        }
+      },
+      "curvature" : -1.996538776374713
+    }, {
+      "time" : 2.2379061099913464,
+      "velocity" : 1.0391587448279456,
+      "acceleration" : 1.6754933878837617,
+      "pose" : {
+        "translation" : {
+          "x" : 1.808834808100432,
+          "y" : 1.8395559468502753
+        },
+        "rotation" : {
+          "radians" : 0.9804301798041166
+        }
+      },
+      "curvature" : -1.8817412716281585
+    }, {
+      "time" : 2.2830950022778635,
+      "velocity" : 1.1148724350597967,
+      "acceleration" : 2.127602917951782,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8376715372041317,
+          "y" : 1.8787621757874542
+        },
+        "rotation" : {
+          "radians" : 0.8948396405471469
+        }
+      },
+      "curvature" : -1.6348328210818963
+    }, {
+      "time" : 2.3276395725958383,
+      "velocity" : 1.2096455928472285,
+      "acceleration" : 2.5789601207010184,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8716574925150553,
+          "y" : 1.9178176341376911
+        },
+        "rotation" : {
+          "radians" : 0.8166729724145454
+        }
+      },
+      "curvature" : -1.3886967164626418
+    }, {
+      "time" : 2.371283333319735,
+      "velocity" : 1.3222011112715757,
+      "acceleration" : 3.0313394226052046,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9109165188377353,
+          "y" : 1.9566924759531596
+        },
+        "rotation" : {
+          "radians" : 0.7463747869451778
+        }
+      },
+      "curvature" : -1.1623279144475585
+    }, {
+      "time" : 2.4138523222192685,
+      "velocity" : 1.4512421655031749,
+      "acceleration" : 3.4847988479105942,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9555249368440535,
+          "y" : 1.9953549631045817
+        },
+        "rotation" : {
+          "radians" : 0.683796860013437
+        }
+      },
+      "curvature" : -0.9648147341420715
+    }, {
+      "time" : 2.455238313142531,
+      "velocity" : 1.5954640189921991,
+      "acceleration" : 3.93825151072416,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0055138388660367,
+          "y" : 2.0337716505743684
+        },
+        "rotation" : {
+          "radians" : 0.6284298787308873
+        }
+      },
+      "curvature" : -0.7982697547383403
+    }, {
+      "time" : 2.49538256040239,
+      "velocity" : 1.7535621614102237,
+      "acceleration" : 4.000000000000021,
+      "pose" : {
+        "translation" : {
+          "x" : 2.060871384688653,
+          "y" : 2.0719075717497617
+        },
+        "rotation" : {
+          "radians" : 0.5795857801198268
+        }
+      },
+      "curvature" : -0.6608172515589942
+    }, {
+      "time" : 2.53441622407071,
+      "velocity" : 1.9096968160835055,
+      "acceleration" : 4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.121545097342608,
+          "y" : 2.1097264237159736
+        },
+        "rotation" : {
+          "radians" : 0.5365211642557431
+        }
+      },
+      "curvature" : -0.5487920239178083
+    }, {
+      "time" : 2.5725848078202302,
+      "velocity" : 2.062371151081586,
+      "acceleration" : 4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 2.18744415889714,
+          "y" : 2.147190752549328
+        },
+        "rotation" : {
+          "radians" : 0.4985109954494367
+        }
+      },
+      "curvature" : -0.45807197306180775
+    }, {
+      "time" : 2.6100585451602245,
+      "velocity" : 2.212266100441563,
+      "acceleration" : 3.9999999999999893,
+      "pose" : {
+        "translation" : {
+          "x" : 2.258441706252816,
+          "y" : 2.1842621386104017
+        },
+        "rotation" : {
+          "radians" : 0.4648869468393921
+        }
+      },
+      "curvature" : -0.3847621795776758
+    }, {
+      "time" : 2.646940241630883,
+      "velocity" : 2.359792886324197,
+      "acceleration" : 4.00000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 2.334377126934328,
+          "y" : 2.220901381837166
+        },
+        "rotation" : {
+          "radians" : 0.43505326862724697
+        }
+      },
+      "curvature" : -0.32547741802200536
+    }, {
+      "time" : 2.683288527639372,
+      "velocity" : 2.505186030358152,
+      "acceleration" : 4.000000000000014,
+      "pose" : {
+        "translation" : {
+          "x" : 2.415058354883288,
+          "y" : 2.2570686870381245
+        },
+        "rotation" : {
+          "radians" : 0.4084896240286113
+        }
+      },
+      "curvature" : -0.2774076507783822
+    }, {
+      "time" : 2.719132405668665,
+      "velocity" : 2.6485615424753246,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.500264166251025,
+          "y" : 2.2927238491854567
+        },
+        "rotation" : {
+          "radians" : 0.384747078396271
+        }
+      },
+      "curvature" : -0.23828130083939184
+    }, {
+      "time" : 2.754480704601807,
+      "velocity" : 2.789954738207894,
+      "acceleration" : 2.9488324318429173,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5897464751913803,
+          "y" : 2.3278264387081586
+        },
+        "rotation" : {
+          "radians" : 0.3634409880883115
+        }
+      },
+      "curvature" : -0.20628975468482194
+    }, {
+      "time" : 2.7895490251950776,
+      "velocity" : 2.8933653393035956,
+      "acceleration" : 0.7037966720327286,
+      "pose" : {
+        "translation" : {
+          "x" : 2.683232629653503,
+          "y" : 2.362335986785183
+        },
+        "rotation" : {
+          "radians" : 0.34424291191521855
+        }
+      },
+      "curvature" : -0.18000474642406875
+    }, {
+      "time" : 2.8249707390558063,
+      "velocity" : 2.918295023636472,
+      "acceleration" : 0.5763378030245723,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7804277071746464,
+          "y" : 2.39621217063858
+        },
+        "rotation" : {
+          "radians" : 0.32687266278130106
+        }
+      },
+      "curvature" : -0.15830253678418688
+    }, {
+      "time" : 2.861139234917162,
+      "velocity" : 2.939140295079909,
+      "acceleration" : 0.4740600472954994,
+      "pose" : {
+        "translation" : {
+          "x" : 2.881016810672964,
+          "y" : 2.429414998826637
+        },
+        "rotation" : {
+          "radians" : 0.31109102748658707
+        }
+      },
+      "curvature" : -0.1402996898246936
+    }, {
+      "time" : 2.8979872681123133,
+      "velocity" : 2.9566084754391486,
+      "acceleration" : 0.3917399621578322,
+      "pose" : {
+        "translation" : {
+          "x" : 2.984667364240305,
+          "y" : 2.4619049965370223
+        },
+        "rotation" : {
+          "radians" : 0.2966933532809512
+        }
+      },
+      "curvature" : -0.1253009626312453
+    }, {
+      "time" : 2.935436818258982,
+      "velocity" : 2.9712789607964325,
+      "acceleration" : 0.3251659903474371,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0910314089350095,
+          "y" : 2.4936433908799245
+        },
+        "rotation" : {
+          "radians" : 0.28350402582744755
+        }
+      },
+      "curvature" : -0.11275805306566417
+    }, {
+      "time" : 2.9734007910904325,
+      "velocity" : 2.9836235536196942,
+      "acceleration" : 0.2709852112638229,
+      "pose" : {
+        "translation" : {
+          "x" : 3.199747898574706,
+          "y" : 2.5245922961811917
+        },
+        "rotation" : {
+          "radians" : 0.2713717786654788
+        }
+      },
+      "curvature" : -0.10223741084606355
+    }, {
+      "time" : 3.0117845646161134,
+      "velocity" : 2.9940249885976535,
+      "acceleration" : 0.1543098408303897,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3104449955291066,
+          "y" : 2.5547148992754742
+        },
+        "rotation" : {
+          "radians" : 0.2601657392085518
+        }
+      },
+      "curvature" : -0.09339533006984713
+    }, {
+      "time" : 3.0505054353667576,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 7.591176208555741E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.422742366512802,
+          "y" : 2.5839756447993656
+        },
+        "rotation" : {
+          "radians" : 0.24977210747664394
+        }
+      },
+      "curvature" : -0.08595876803031433
+    }, {
+      "time" : 3.0895059097464324,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5362534783780575,
+          "y" : 2.612340420484543
+        },
+        "rotation" : {
+          "radians" : 0.24009136790995844
+        }
+      },
+      "curvature" : -0.07971061642849785
+    }, {
+      "time" : 3.1286993195347663,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.650587893907611,
+          "y" : 2.6397767424509064
+        },
+        "rotation" : {
+          "radians" : 0.23103594442528902
+        }
+      },
+      "curvature" : -0.07447841843016513
+    }, {
+      "time" : 3.16795942350856,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.765353567607466,
+          "y" : 2.666253940499722
+        },
+        "rotation" : {
+          "radians" : 0.22252822046257315
+        }
+      },
+      "curvature" : -0.07012575415648367
+    }, {
+      "time" : 3.2071598053906003,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8801591414996897,
+          "y" : 2.6917433434067615
+        },
+        "rotation" : {
+          "radians" : 0.21449885716390527
+        }
+      },
+      "curvature" : -0.06654570394142899
+    }, {
+      "time" : 3.2461747016130165,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9946162409152066,
+          "y" : 2.7162184642154443
+        },
+        "rotation" : {
+          "radians" : 0.20688535313560402
+        }
+      },
+      "curvature" : -0.06365594712905223
+    }, {
+      "time" : 3.2848798203338982,
+      "velocity" : 3.0,
+      "acceleration" : -7.735398279518994E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.1083417702865965,
+          "y" : 2.7396551855299762
+        },
+        "rotation" : {
+          "radians" : 0.19963079814758483
+        }
+      },
+      "curvature" : -0.06139517108104972
+    }, {
+      "time" : 3.32315315320381,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 7.84832578379943E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.220960208940891,
+          "y" : 2.7620319448084913
+        },
+        "rotation" : {
+          "radians" : 0.19268278060685093
+        }
+      },
+      "curvature" : -0.05972055737308964
+    }, {
+      "time" : 3.3608757811105803,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.332105906892365,
+          "y" : 2.783329919656194
+        },
+        "rotation" : {
+          "radians" : 0.18599241484195372
+        }
+      },
+      "curvature" : -0.0586061860352566
+    }, {
+      "time" : 3.3979326749154137,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.441425380635337,
+          "y" : 2.8035332131184987
+        },
+        "rotation" : {
+          "radians" : 0.17951345937534083
+        }
+      },
+      "curvature" : -0.05804225916920248
+    }, {
+      "time" : 3.43421349202006,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.548579608936964,
+          "y" : 2.8226290389741697
+        },
+        "rotation" : {
+          "radians" : 0.17320150172377563
+        }
+      },
+      "curvature" : -0.058035096135246704
+    }, {
+      "time" : 3.4696133694646405,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.653246328630036,
+          "y" : 2.8406079070284633
+        },
+        "rotation" : {
+          "radians" : 0.16701318920823915
+        }
+      },
+      "curvature" : -0.05860789609869524
+    }, {
+      "time" : 3.5040337141415066,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.755122330405774,
+          "y" : 2.857463808406269
+        },
+        "rotation" : {
+          "radians" : 0.16090548924261228
+        }
+      },
+      "curvature" : -0.05980230049917739
+    }, {
+      "time" : 3.5373829906159067,
+      "velocity" : 3.0,
+      "acceleration" : -1.3000955620508416,
+      "pose" : {
+        "translation" : {
+          "x" : 4.853925754606621,
+          "y" : 2.87319440084525
+        },
+        "rotation" : {
+          "radians" : 0.15483496726687765
+        }
+      },
+      "curvature" : -0.06168081541885887
+    }, {
+      "time" : 3.5698052848244366,
+      "velocity" : 2.9578479191879836,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.9493983870190466,
+          "y" : 2.8878011939889827
+        },
+        "rotation" : {
+          "radians" : 0.14875707688110845
+        }
+      },
+      "curvature" : -0.0643301641863426
+    }, {
+      "time" : 3.601908104387069,
+      "velocity" : 2.8294366409374536,
+      "acceleration" : -3.999999999999995,
+      "pose" : {
+        "translation" : {
+          "x" : 5.041307954666333,
+          "y" : 2.9012897346800983
+        },
+        "rotation" : {
+          "radians" : 0.1426254663359473
+        }
+      },
+      "curvature" : -0.06786561693146706
+    }, {
+      "time" : 3.634098277881151,
+      "velocity" : 2.700675946961127,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.12945042160138,
+          "y" : 2.913669792253425
+        },
+        "rotation" : {
+          "radians" : 0.13639132070467594
+        }
+      },
+      "curvature" : -0.07243625256641337
+    }, {
+      "time" : 3.6663242273354184,
+      "velocity" : 2.5717721491440564,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.213652284699492,
+          "y" : 2.924955543829128
+        },
+        "rotation" : {
+          "radians" : 0.13000278348350505
+        }
+      },
+      "curvature" : -0.07823088630580577
+    }, {
+      "time" : 3.6985369799811316,
+      "velocity" : 2.4429211385612026,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 5.293772869451182,
+          "y" : 2.935165759605849
+        },
+        "rotation" : {
+          "radians" : 0.12340454072534943
+        }
+      },
+      "curvature" : -0.08548392661118162
+    }, {
+      "time" : 3.730691893404645,
+      "velocity" : 2.314301484867149,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.3697066257549615,
+          "y" : 2.9443239881538474
+        },
+        "rotation" : {
+          "radians" : 0.11653771369919076
+        }
+      },
+      "curvature" : -0.09447950544669352
+    }, {
+      "time" : 3.7627510266183024,
+      "velocity" : 2.1860649520125195,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.441385423710139,
+          "y" : 2.952458741708144
+        },
+        "rotation" : {
+          "radians" : 0.10934030498899684
+        }
+      },
+      "curvature" : -0.10555051064633172
+    }, {
+      "time" : 3.7946864190689498,
+      "velocity" : 2.058323382209931,
+      "acceleration" : -3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 5.508780849409618,
+          "y" : 2.959603681461658
+        },
+        "rotation" : {
+          "radians" : 0.10174859495289124
+        }
+      },
+      "curvature" : -0.1190660934873409
+    }, {
+      "time" : 3.8581528018765208,
+      "velocity" : 1.8044578509796478,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.630820283137819,
+          "y" : 2.971085620886363
+        },
+        "rotation" : {
+          "radians" : 0.08513910857652777
+        }
+      },
+      "curvature" : -0.15483239704424728
+    }, {
+      "time" : 3.9212966580627038,
+      "velocity" : 1.5518824262349158,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.73647917568087,
+          "y" : 2.9791491162686725
+        },
+        "rotation" : {
+          "radians" : 0.0663518340621249
+        }
+      },
+      "curvature" : -0.20277252875109172
+    }, {
+      "time" : 3.985088994027884,
+      "velocity" : 1.296713082374195,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.827194162416493,
+          "y" : 2.98426771953671
+        },
+        "rotation" : {
+          "radians" : 0.045585623619075005
+        }
+      },
+      "curvature" : -0.2546275827711898
+    }, {
+      "time" : 4.0522933348683985,
+      "velocity" : 1.027895719012139,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.9052577307282945,
+          "y" : 2.987015154086521
+        },
+        "rotation" : {
+          "radians" : 0.02445962652264226
+        }
+      },
+      "curvature" : -0.27725843604659994
+    }, {
+      "time" : 4.131181440085891,
+      "velocity" : 0.7123432981421679,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.973891685375232,
+          "y" : 2.988071244162576
+        },
+        "rotation" : {
+          "radians" : 0.007123366020833881
+        }
+      },
+      "curvature" : -0.20476533171945074
+    }, {
+      "time" : 4.309267264621433,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.037320613861084,
+          "y" : 2.9882278442382812
+        },
+        "rotation" : {
+          "radians" : 0.0
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 4.056426048278809, -3.3782451152801514, 0.0 ],
+      "y" : [ -0.5667314529418945, 2.785569906234741, 0.0 ]
+    }, {
+      "x" : [ 1.7378531694412231, -0.24853384494781494, 0.0 ],
+      "y" : [ 1.444043517112732, 2.535710334777832, 0.0 ]
+    }, {
+      "x" : [ 6.037320613861084, 2.0, 0.0 ],
+      "y" : [ 2.9882278442382812, 0.0, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.2873733127254927
+      }
+    }, {
+      "time" : 1.8499999586492777,
+      "rotation" : {
+        "radians" : 2.3228735180023055
+      }
+    }, {
+      "time" : 4.21999990567565,
+      "rotation" : {
+        "radians" : 0.6128208180837543
+      }
+    } ],
+    "reversed" : false,
+    "color" : 124.23529,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  } ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/autos/blue/hidecargo.json b/src/main/deploy/autos/blue/hidecargo.json
new file mode 100644
index 00000000..16cbc83b
--- /dev/null
+++ b/src/main/deploy/autos/blue/hidecargo.json
@@ -0,0 +1,3175 @@
+{
+  "autonomousSteps" : [ {
+    "type" : "script",
+    "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "OPEN" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "INTAKE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "ON" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.1293768882751465,
+          "y" : 1.0937262773513794
+        },
+        "rotation" : {
+          "radians" : 2.4178655244299407
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.24452127463632814,
+      "velocity" : 0.9780850985453124,
+      "acceleration" : 3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.039938468317359,
+          "y" : 1.173101702633744
+        },
+        "rotation" : {
+          "radians" : 2.411607121974174
+        }
+      },
+      "curvature" : -0.10063347960741297
+    }, {
+      "time" : 0.3430023265109939,
+      "velocity" : 1.3720093060439753,
+      "acceleration" : 3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.954285601692391,
+          "y" : 1.2509135147447523
+        },
+        "rotation" : {
+          "radians" : 2.395009715228919
+        }
+      },
+      "curvature" : -0.1855623376134662
+    }, {
+      "time" : 0.4150553084726476,
+      "velocity" : 1.6602212338905902,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.87496604004582,
+          "y" : 1.3260262403808838
+        },
+        "rotation" : {
+          "radians" : 2.3703948601089895
+        }
+      },
+      "curvature" : -0.26531768438500725
+    }, {
+      "time" : 0.4721670734641104,
+      "velocity" : 1.8886682938564414,
+      "acceleration" : 3.999999999999993,
+      "pose" : {
+        "translation" : {
+          "x" : 5.803342186380178,
+          "y" : 1.3977211365709081
+        },
+        "rotation" : {
+          "radians" : 2.339767645326564
+        }
+      },
+      "curvature" : -0.3381985496587611
+    }, {
+      "time" : 0.5190911939858679,
+      "velocity" : 2.076364775943471,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.7397725735722815,
+          "y" : 1.4656408262000014
+        },
+        "rotation" : {
+          "radians" : 2.3056558386592805
+        }
+      },
+      "curvature" : -0.3907488038191573
+    }, {
+      "time" : 0.5585735987097502,
+      "velocity" : 2.2342943948390004,
+      "acceleration" : 0.26324528204804704,
+      "pose" : {
+        "translation" : {
+          "x" : 5.683793342890567,
+          "y" : 1.529733933533862
+        },
+        "rotation" : {
+          "radians" : 2.271678768601324
+        }
+      },
+      "curvature" : -0.39766870774643054
+    }, {
+      "time" : 0.5934744799766615,
+      "velocity" : 2.243481887171834,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.634299722512424,
+          "y" : 1.5901997197428273
+        },
+        "rotation" : {
+          "radians" : 2.242697343821613
+        }
+      },
+      "curvature" : -0.32812227024366547
+    }, {
+      "time" : 0.6267989018195489,
+      "velocity" : 2.110184199800284,
+      "acceleration" : -3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 5.589727506041527,
+          "y" : 1.6474327184259892
+        },
+        "rotation" : {
+          "radians" : 2.224227521884603
+        }
+      },
+      "curvature" : -0.16341344710143665
+    }, {
+      "time" : 0.6603386127840065,
+      "velocity" : 1.9760253559424537,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.548234531025173,
+          "y" : 1.7019673711353107
+        },
+        "rotation" : {
+          "radians" : 2.2209763317101947
+        }
+      },
+      "curvature" : 0.07956164338822755
+    }, {
+      "time" : 0.6950498794552016,
+      "velocity" : 1.8371802892576732,
+      "acceleration" : -4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 5.507882157471613,
+          "y" : 1.7544226628997421
+        },
+        "rotation" : {
+          "radians" : 2.234944043969041
+        }
+      },
+      "curvature" : 0.33953937734428047
+    }, {
+      "time" : 0.732203292039014,
+      "velocity" : 1.6885666389224232,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.466816746367385,
+          "y" : 1.805446757749337
+        },
+        "rotation" : {
+          "radians" : 2.264150133653273
+        }
+      },
+      "curvature" : 0.5355105351904276
+    }, {
+      "time" : 0.773517717518194,
+      "velocity" : 1.5233089370057031,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.42345113819465,
+          "y" : 1.8556616342393681
+        },
+        "rotation" : {
+          "radians" : 2.30296870864004
+        }
+      },
+      "curvature" : 0.6126486386243994
+    }, {
+      "time" : 0.8214715782483804,
+      "velocity" : 1.3314934940849577,
+      "acceleration" : -3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 5.376646131448524,
+          "y" : 1.9056077209744444
+        },
+        "rotation" : {
+          "radians" : 2.343996330815072
+        }
+      },
+      "curvature" : 0.568293999561127
+    }, {
+      "time" : 0.8802038597478065,
+      "velocity" : 1.0965643680872534,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.3258919611544115,
+          "y" : 1.9556885321326263
+        },
+        "rotation" : {
+          "radians" : 2.3801609988469874
+        }
+      },
+      "curvature" : 0.4354667748411578
+    }, {
+      "time" : 0.9592448110914289,
+      "velocity" : 0.7804005627127639,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.271489777385341,
+          "y" : 2.006115302989542
+        },
+        "rotation" : {
+          "radians" : 2.405692727133013
+        }
+      },
+      "curvature" : 0.24590943352090008
+    }, {
+      "time" : 1.1543449517696198,
+      "velocity" : 0.0,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.214733123779297,
+          "y" : 2.056851625442505
+        },
+        "rotation" : {
+          "radians" : 2.4154850346736763
+        }
+      },
+      "curvature" : -0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 6.1293768882751465, -1.4423890113830566, 0.0 ],
+      "y" : [ 1.0937262773513794, 1.27461576461792, 0.0 ]
+    }, {
+      "x" : [ 5.214733123779297, -0.9154930114746094, 0.0 ],
+      "y" : [ 2.056851625442505, 0.812895655632019, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.37094272169677
+      }
+    }, {
+      "time" : 1.0699999760836363,
+      "rotation" : {
+        "radians" : 2.370942522371697
+      }
+    } ],
+    "reversed" : false,
+    "color" : 209.88235,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 7",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.VisionManager.shootBalls",
+        "args" : [ "7" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "Hopper.setEjectOverride true\nDrive.turnToAngle 45",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setEjectOverride",
+        "args" : [ "true" ],
+        "argTypes" : [ "boolean" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Drive.turnToAngle",
+        "args" : [ "45" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.214733123779297,
+          "y" : 2.056851625442505
+        },
+        "rotation" : {
+          "radians" : 0.9530775920796433
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.22164892589636578,
+      "velocity" : 0.8865957035854634,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.272283410420641,
+          "y" : 2.136490201934066
+        },
+        "rotation" : {
+          "radians" : 0.93154333853559
+        }
+      },
+      "curvature" : -0.3354703724748357
+    }, {
+      "time" : 0.32212126730993046,
+      "velocity" : 1.2884850692397223,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 5.339010516181588,
+          "y" : 2.223017249489203
+        },
+        "rotation" : {
+          "radians" : 0.8980578131179813
+        }
+      },
+      "curvature" : -0.24297628723974596
+    }, {
+      "time" : 0.40798632695507736,
+      "velocity" : 1.6319453078203097,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.418256742414087,
+          "y" : 2.320179727881623
+        },
+        "rotation" : {
+          "radians" : 0.8787668506583872
+        }
+      },
+      "curvature" : -0.07195280186555318
+    }, {
+      "time" : 0.4482103474158235,
+      "velocity" : 1.7928413896632942,
+      "acceleration" : 4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 5.462294308261335,
+          "y" : 2.373142516471262
+        },
+        "rotation" : {
+          "radians" : 0.8763509051285007
+        }
+      },
+      "curvature" : -0.0010310805010032027
+    }, {
+      "time" : 0.48701783511066765,
+      "velocity" : 1.9480713404426708,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.508707731962204,
+          "y" : 2.428952641785145
+        },
+        "rotation" : {
+          "radians" : 0.8784798678297706
+        }
+      },
+      "curvature" : 0.057187573384384974
+    }, {
+      "time" : 0.5244749433253648,
+      "velocity" : 2.0978997733014593,
+      "acceleration" : -3.006834100609329,
+      "pose" : {
+        "translation" : {
+          "x" : 5.5569134053257585,
+          "y" : 2.4874170951864016
+        },
+        "rotation" : {
+          "radians" : 0.8846611837531144
+        }
+      },
+      "curvature" : 0.10384088533571874
+    }, {
+      "time" : 0.5628765788352517,
+      "velocity" : 1.9824324261311614,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 5.606271566124633,
+          "y" : 2.5482597037116648
+        },
+        "rotation" : {
+          "radians" : 0.8942836471940969
+        }
+      },
+      "curvature" : 0.13988861038856043
+    }, {
+      "time" : 0.6051667706790917,
+      "velocity" : 1.813271658755801,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.656145019875112,
+          "y" : 2.6111436507878807
+        },
+        "rotation" : {
+          "radians" : 0.9066044762714179
+        }
+      },
+      "curvature" : 0.16515378657020438
+    }, {
+      "time" : 0.6526161179857626,
+      "velocity" : 1.6234742695291178,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.7059578616172075,
+          "y" : 2.6756939969491214
+        },
+        "rotation" : {
+          "radians" : 0.9206836126942823
+        }
+      },
+      "curvature" : 0.17781422207911715
+    }, {
+      "time" : 0.7069026874415512,
+      "velocity" : 1.4063279917059635,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.75525419769474,
+          "y" : 2.7415202005533956
+        },
+        "rotation" : {
+          "radians" : 0.9352834691742239
+        }
+      },
+      "curvature" : 0.1740904256082478
+    }, {
+      "time" : 0.7714879126896856,
+      "velocity" : 1.1479870907134255,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.803756867535412,
+          "y" : 2.8082386384994606
+        },
+        "rotation" : {
+          "radians" : 0.9487507880978892
+        }
+      },
+      "curvature" : 0.14816445891139096
+    }, {
+      "time" : 0.8556329295191124,
+      "velocity" : 0.8114070233957184,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.851426165430894,
+          "y" : 2.8754951269436333
+        },
+        "rotation" : {
+          "radians" : 0.9589069733276255
+        }
+      },
+      "curvature" : 0.09262519823750537
+    }, {
+      "time" : 1.058484685368042,
+      "velocity" : 0.0,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.8985185623168945,
+          "y" : 2.9429874420166016
+        },
+        "rotation" : {
+          "radians" : 0.9629949901604784
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.214733123779297, 0.4449276924133301, 0.0 ],
+      "y" : [ 2.056851625442505, 0.6262435913085938, 0.0 ]
+    }, {
+      "x" : [ 5.8985185623168945, 0.7516036033630371, 0.0 ],
+      "y" : [ 2.9429874420166016, 1.0804316997528076, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.9599310885968813
+      }
+    }, {
+      "time" : 0.9699999783188105,
+      "rotation" : {
+        "radians" : 0.9599310885968813
+      }
+    } ],
+    "reversed" : false,
+    "color" : 148.47061,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@t 1.3\nIntake.setIntakeSolState CLOSE\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 1.3,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "CLOSE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "OFF" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "OFF" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "@t 3.36\nIntake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 3.36,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "OPEN" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "INTAKE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "ON" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.8985185623168945,
+          "y" : 2.9429874420166016
+        },
+        "rotation" : {
+          "radians" : -3.1024027203970066
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.2301838401073264,
+      "velocity" : 0.9207353604293056,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.792630460413637,
+          "y" : 2.9388424054094813
+        },
+        "rotation" : {
+          "radians" : -3.1025922709616696
+        }
+      },
+      "curvature" : -0.0034322907229005963
+    }, {
+      "time" : 0.3255946694282438,
+      "velocity" : 1.302378677712975,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.686655575860993,
+          "y" : 2.9347317908029744
+        },
+        "rotation" : {
+          "radians" : -3.1031002310232063
+        }
+      },
+      "curvature" : -0.006005316476928024
+    }, {
+      "time" : 0.3988940171844041,
+      "velocity" : 1.5955760687376164,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.580523715266813,
+          "y" : 2.930682060104536
+        },
+        "rotation" : {
+          "radians" : -3.103837861955969
+        }
+      },
+      "curvature" : -0.007749412153373173
+    }, {
+      "time" : 0.4607866040428335,
+      "velocity" : 1.843146416171334,
+      "acceleration" : 3.999999999999994,
+      "pose" : {
+        "translation" : {
+          "x" : 5.4741821068677154,
+          "y" : 2.926711325853944
+        },
+        "rotation" : {
+          "radians" : -3.104720238709075
+        }
+      },
+      "curvature" : -0.008706622259397081
+    }, {
+      "time" : 0.5154157029130185,
+      "velocity" : 2.0616628116520737,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.367594657212031,
+          "y" : 2.9228297190306436
+        },
+        "rotation" : {
+          "radians" : -3.1056669054798047
+        }
+      },
+      "curvature" : -0.00892606605401968
+    }, {
+      "time" : 0.5649014668690089,
+      "velocity" : 2.259605867476035,
+      "acceleration" : 4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 5.260741207842759,
+          "y" : 2.9190397568610926
+        },
+        "rotation" : {
+          "radians" : -3.106602230083352
+        }
+      },
+      "curvature" : -0.008459920513905771
+    }, {
+      "time" : 0.6104980484072738,
+      "velocity" : 2.441992193629095,
+      "acceleration" : 3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 5.153616791980521,
+          "y" : 2.9153367106261046
+        },
+        "rotation" : {
+          "radians" : -3.1074555011063474
+        }
+      },
+      "curvature" : -0.007360079879286901
+    }, {
+      "time" : 0.6530171873993678,
+      "velocity" : 2.6120687495974706,
+      "acceleration" : 3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 5.0462308912065055,
+          "y" : 2.911708973468194
+        },
+        "rotation" : {
+          "radians" : -3.108160812688321
+        }
+      },
+      "curvature" : -0.005675482209767876
+    }, {
+      "time" : 0.6930174258760853,
+      "velocity" : 2.7720697035043407,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.93860669214543,
+          "y" : 2.9081384281989218
+        },
+        "rotation" : {
+          "radians" : -3.108656779571012
+        }
+      },
+      "curvature" : -0.003450046167250236
+    }, {
+      "time" : 0.7309003592448649,
+      "velocity" : 2.923601436979459,
+      "acceleration" : 2.0944239970255154,
+      "pose" : {
+        "translation" : {
+          "x" : 4.830780343148486,
+          "y" : 2.904600815106239
+        },
+        "rotation" : {
+          "radians" : -3.108886120874728
+        }
+      },
+      "curvature" : -7.211325069651598E-4
+    }, {
+      "time" : 0.7673774828497517,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.722800210976294,
+          "y" : 2.9010660997618305
+        },
+        "rotation" : {
+          "radians" : -3.10879514573791
+        }
+      },
+      "curvature" : 0.0024815692229093363
+    }, {
+      "time" : 0.8034217930772413,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.614726137481853,
+          "y" : 2.8974988408284617
+        },
+        "rotation" : {
+          "radians" : -3.1083331681482385
+        }
+      },
+      "curvature" : 0.0061368314589584135
+    }, {
+      "time" : 0.839474699342444,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.506628696293496,
+          "y" : 2.8938585578673215
+        },
+        "rotation" : {
+          "radians" : -3.107451872483656
+        }
+      },
+      "curvature" : 0.010232455610621435
+    }, {
+      "time" : 0.8755098996327016,
+      "velocity" : 3.0,
+      "acceleration" : -8.22656662136612E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.39858844949784,
+          "y" : 2.890100099145368
+        },
+        "rotation" : {
+          "radians" : -3.1061046457957544
+        }
+      },
+      "curvature" : 0.01476570595533646
+    }, {
+      "time" : 0.9114981176657808,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.2906952043227395,
+          "y" : 2.886174009442671
+        },
+        "rotation" : {
+          "radians" : -3.1042458879032835
+        }
+      },
+      "curvature" : 0.019743691371007215
+    }, {
+      "time" : 0.9474073803784949,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.270759764362102E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.183047269820236,
+          "y" : 2.8820268978597596
+        },
+        "rotation" : {
+          "radians" : -3.1018303060188313
+        }
+      },
+      "curvature" : 0.025183766010214763
+    }, {
+      "time" : 0.9832033025994578,
+      "velocity" : 3.0,
+      "acceleration" : -8.305527617360462E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.075750713549512,
+          "y" : 2.8776018056249644
+        },
+        "rotation" : {
+          "radians" : -3.0988121969366995
+        }
+      },
+      "curvature" : 0.031114011207342505
+    }, {
+      "time" : 1.0188493791490731,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.349604098090194E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9689186182598455,
+          "y" : 2.8728385739017632
+        },
+        "rotation" : {
+          "radians" : -3.095144716752075
+        }
+      },
+      "curvature" : 0.03757385368866147
+    }, {
+      "time" : 1.0543072846683412,
+      "velocity" : 3.0,
+      "acceleration" : -8.403642908942724E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8626703385735555,
+          "y" : 2.867674211596125
+        },
+        "rotation" : {
+          "radians" : -3.0907791356286958
+        }
+      },
+      "curvature" : 0.0446148697110529
+    }, {
+      "time" : 1.0895371815644033,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7571307576689605,
+          "y" : 2.862043263163855
+        },
+        "rotation" : {
+          "radians" : -3.0856640732526293
+        }
+      },
+      "curvature" : 0.05230182094645797
+    }, {
+      "time" : 1.124498036572409,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.544307135767894E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6524295439633274,
+          "y" : 2.855878176417939
+        },
+        "rotation" : {
+          "radians" : -3.0797447092896437
+        }
+      },
+      "curvature" : 0.06071396531738544
+    }, {
+      "time" : 1.159147946568507,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.548700407795824,
+          "y" : 2.8491096703358885
+        },
+        "rotation" : {
+          "radians" : -3.0729619624239923
+        }
+      },
+      "curvature" : 0.0699466838117423
+    }, {
+      "time" : 1.1934444744263788,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4460803581104718,
+          "y" : 2.841667102867084
+        },
+        "rotation" : {
+          "radians" : -3.0652516314712313
+        }
+      },
+      "curvature" : 0.08011346138708186
+    }, {
+      "time" : 1.2273449958888158,
+      "velocity" : 3.0,
+      "acceleration" : -0.16623210994932952,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3447089591390977,
+          "y" : 2.8334788387401204
+        },
+        "rotation" : {
+          "radians" : -3.0565434927756256
+        }
+      },
+      "curvature" : 0.09134825465286359
+    }, {
+      "time" : 1.2608381382277525,
+      "velocity" : 2.9944323642801653,
+      "acceleration" : -0.4439832537480419,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2447275870842858,
+          "y" : 2.8244726172701524
+        },
+        "rotation" : {
+          "radians" : -3.046760349874518
+        }
+      },
+      "curvature" : 0.10380826846683365
+    }, {
+      "time" : 1.2939625007283606,
+      "velocity" : 2.9797257020388157,
+      "acceleration" : -0.504652005340554,
+      "pose" : {
+        "translation" : {
+          "x" : 3.1462786868023294,
+          "y" : 2.814575920166238
+        },
+        "rotation" : {
+          "radians" : -3.0358170346264064
+        }
+      },
+      "curvature" : 0.11767714398773378
+    }, {
+      "time" : 1.3267346665866735,
+      "velocity" : 2.963187162819065,
+      "acceleration" : -0.5754544680210432,
+      "pose" : {
+        "translation" : {
+          "x" : 3.049505028486184,
+          "y" : 2.8037163393386826
+        },
+        "rotation" : {
+          "radians" : -3.023619364229805
+        }
+      },
+      "curvature" : 0.13316852630405346
+    }, {
+      "time" : 1.3591322565139792,
+      "velocity" : 2.9445438249422833,
+      "acceleration" : -0.6578485083677343,
+      "pose" : {
+        "translation" : {
+          "x" : 2.954548964348419,
+          "y" : 2.791821944706385
+        },
+        "rotation" : {
+          "radians" : -3.010063066623661
+        }
+      },
+      "curvature" : 0.1505299220368132
+    }, {
+      "time" : 1.391136692414039,
+      "velocity" : 2.923489754524278,
+      "acceleration" : -0.7533080854904565,
+      "pose" : {
+        "translation" : {
+          "x" : 2.8615516853041685,
+          "y" : 2.7788216520041806
+        },
+        "rotation" : {
+          "radians" : -2.995032698790502
+        }
+      },
+      "curvature" : 0.17004666410797128
+    }, {
+      "time" : 1.4227338667361988,
+      "velocity" : 2.8996873476287437,
+      "acceleration" : -0.8632025105219969,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7706524776540853,
+          "y" : 2.764645590590188
+        },
+        "rotation" : {
+          "radians" : -2.9784006000086296
+        }
+      },
+      "curvature" : 0.19204565508070712
+    }, {
+      "time" : 1.4539148256972156,
+      "velocity" : 2.8727718655731107,
+      "acceleration" : -3.0556739243578237,
+      "pose" : {
+        "translation" : {
+          "x" : 2.681987979767292,
+          "y" : 2.749225471253151
+        },
+        "rotation" : {
+          "radians" : -2.9600259471131536
+        }
+      },
+      "curvature" : 0.2168983389890578
+    }, {
+      "time" : 1.4850284729965166,
+      "velocity" : 2.7776987048289703,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5956914387643337,
+          "y" : 2.732494954019785
+        },
+        "rotation" : {
+          "radians" : -2.9397540138154308
+        }
+      },
+      "curvature" : 0.24502202477503635
+    }, {
+      "time" : 1.516611424846333,
+      "velocity" : 2.6513668974297047,
+      "acceleration" : -4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5118919672001288,
+          "y" : 2.7143900159621204
+        },
+        "rotation" : {
+          "radians" : -2.917415783997239
+        }
+      },
+      "curvature" : 0.2768782177504529
+    }, {
+      "time" : 1.548889355951825,
+      "velocity" : 2.5222551730077356,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.430713799746922,
+          "y" : 2.694849319004849
+        },
+        "rotation" : {
+          "radians" : -2.8928281326387415
+        }
+      },
+      "curvature" : 0.31296597525025704
+    }, {
+      "time" : 1.5819534964682787,
+      "velocity" : 2.3899986109419213,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3522755498772367,
+          "y" : 2.673814577732666
+        },
+        "rotation" : {
+          "radians" : -2.8657948699982057
+        }
+      },
+      "curvature" : 0.35380747084108527
+    }, {
+      "time" : 1.6159267882434154,
+      "velocity" : 2.254105443841374,
+      "acceleration" : -3.8097016292232664,
+      "pose" : {
+        "translation" : {
+          "x" : 2.276689466546827,
+          "y" : 2.6512309271976164
+        },
+        "rotation" : {
+          "radians" : -2.8361090449991986
+        }
+      },
+      "curvature" : 0.3999219560210479
+    }, {
+      "time" : 1.6509216012001868,
+      "velocity" : 2.120785647905598,
+      "acceleration" : -3.4461347634327937,
+      "pose" : {
+        "translation" : {
+          "x" : 2.204060690877629,
+          "y" : 2.62704729072644
+        },
+        "rotation" : {
+          "radians" : -2.80355701776569
+        }
+      },
+      "curvature" : 0.45178327798480244
+    }, {
+      "time" : 1.686971311060505,
+      "velocity" : 1.9965534895442902,
+      "acceleration" : -3.10238739377085,
+      "pose" : {
+        "translation" : {
+          "x" : 2.134486512840713,
+          "y" : 2.6012167477279142
+        },
+        "rotation" : {
+          "radians" : -2.7679249248735176
+        }
+      },
+      "curvature" : 0.5097553634159661
+    }, {
+      "time" : 1.7240545520371775,
+      "velocity" : 1.8815069102180944,
+      "acceleration" : -2.774924611544726,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0680556279392364,
+          "y" : 2.5736969015002
+        },
+        "rotation" : {
+          "radians" : -2.7290082446458435
+        }
+      },
+      "curvature" : 0.5740002323865068
+    }, {
+      "time" : 1.7621406045083514,
+      "velocity" : 1.7758209858592497,
+      "acceleration" : -2.460804023554924,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0048473938913958,
+          "y" : 2.544450247038185
+        },
+        "rotation" : {
+          "radians" : -2.6866251713458205
+        }
+      },
+      "curvature" : 0.6443551970164934
+    }, {
+      "time" : 1.8011869660776936,
+      "velocity" : 1.6797355422042324,
+      "acceleration" : -2.1575333813881774,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9449310873133778,
+          "y" : 2.5134445388408304
+        },
+        "rotation" : {
+          "radians" : -2.640634345688177
+        }
+      },
+      "curvature" : 0.7201813658904849
+    }, {
+      "time" : 1.841136715487461,
+      "velocity" : 1.5935426242745667,
+      "acceleration" : -1.86294906736094,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8883651604023122,
+          "y" : 2.480653158718514
+        },
+        "rotation" : {
+          "radians" : -2.590957058758506
+        }
+      },
+      "curvature" : 0.8001959234416516
+    }, {
+      "time" : 1.8819158229901707,
+      "velocity" : 1.5175732239845823,
+      "acceleration" : -1.5751064136624524,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8351964976192239,
+          "y" : 2.4460554836003747
+        },
+        "rotation" : {
+          "radians" : -2.537603239437523
+        }
+      },
+      "curvature" : 0.8823164840063602
+    }, {
+      "time" : 1.9234306511360026,
+      "velocity" : 1.452182951909988,
+      "acceleration" : -1.292173326268649,
+      "pose" : {
+        "translation" : {
+          "x" : 1.785459672371985,
+          "y" : 2.409637253341657
+        },
+        "rotation" : {
+          "radians" : -2.480699299573502
+        }
+      },
+      "curvature" : 0.9635650423352979
+    }, {
+      "time" : 1.9655659800403293,
+      "velocity" : 1.3977368038062605,
+      "acceleration" : -1.0123180093738429,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7391762036982668,
+          "y" : 2.3713909385310568
+        },
+        "rotation" : {
+          "radians" : -2.420514345602534
+        }
+      },
+      "curvature" : 1.0400947450439444
+    }, {
+      "time" : 2.008183951432712,
+      "velocity" : 1.3545938638427721,
+      "acceleration" : -0.733579462931417,
+      "pose" : {
+        "translation" : {
+          "x" : 1.696353812948491,
+          "y" : 2.331316108298065
+        },
+        "rotation" : {
+          "radians" : -2.357479728094081
+        }
+      },
+      "curvature" : 1.1074024899538168
+    }, {
+      "time" : 2.051124315824919,
+      "velocity" : 1.3230936943938578,
+      "acceleration" : -0.4537052298796883,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6569856804687841,
+          "y" : 2.2894197981203117
+        },
+        "rotation" : {
+          "radians" : -2.2921960475647642
+        }
+      },
+      "curvature" : 1.1607601882406264
+    }, {
+      "time" : 2.094206248814145,
+      "velocity" : 1.3035471960833196,
+      "acceleration" : -0.16993335972179005,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6210497022839263,
+          "y" : 2.2457168776309118
+        },
+        "rotation" : {
+          "radians" : -2.2254223695416244
+        }
+      },
+      "curvature" : 1.1958320363685782
+    }, {
+      "time" : 2.137231762276557,
+      "velocity" : 1.2962357260268969,
+      "acceleration" : 0.12131769723435752,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5885077467803068,
+          "y" : 2.200230418425809
+        },
+        "rotation" : {
+          "radians" : -2.1580450690428172
+        }
+      },
+      "curvature" : 1.2093603605633507
+    }, {
+      "time" : 2.1799904071245306,
+      "velocity" : 1.3014231063567148,
+      "acceleration" : 0.42491135071399705,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5593049113888746,
+          "y" : 2.152992061871121
+        },
+        "rotation" : {
+          "radians" : -2.091028180878493
+        }
+      },
+      "curvature" : 1.1997387249442901
+    }, {
+      "time" : 2.2222646301244318,
+      "velocity" : 1.3193859035519875,
+      "acceleration" : 0.7477114243725372,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5333687792680886,
+          "y" : 2.1040423869104825
+        },
+        "rotation" : {
+          "radians" : -2.0253520831348943
+        }
+      },
+      "curvature" : 1.1672933843552342
+    }, {
+      "time" : 2.2638349143884384,
+      "velocity" : 1.350468480010599,
+      "acceleration" : 1.099745716491029,
+      "pose" : {
+        "translation" : {
+          "x" : 1.510608675986873,
+          "y" : 2.0534312778723915
+        },
+        "rotation" : {
+          "radians" : -1.9619508454178411
+        }
+      },
+      "curvature" : 1.1141785653908227
+    }, {
+      "time" : 2.3044837782843635,
+      "velocity" : 1.3951718939603694,
+      "acceleration" : 1.4961728408552466,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4909149262075676,
+          "y" : 2.0012182922775534
+        },
+        "rotation" : {
+          "radians" : -1.9016590628319923
+        }
+      },
+      "curvature" : 1.0439225192180022
+    }, {
+      "time" : 2.343997831668839,
+      "velocity" : 1.4542917474663264,
+      "acceleration" : 1.9607687708811097,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4741581103688794,
+          "y" : 1.9474730286462254
+        },
+        "rotation" : {
+          "radians" : -1.8451762474768723
+        }
+      },
+      "curvature" : 0.9607726336453983
+    }, {
+      "time" : 2.3821673081822796,
+      "velocity" : 1.5291332650147609,
+      "acceleration" : 2.532455282243196,
+      "pose" : {
+        "translation" : {
+          "x" : 1.460188321368836,
+          "y" : 1.8922754943055606
+        },
+        "rotation" : {
+          "radians" : -1.7930521282820115
+        }
+      },
+      "curvature" : 0.869026522779364
+    }, {
+      "time" : 2.4187826706759017,
+      "velocity" : 1.6218600331729836,
+      "acceleration" : 3.8046696696170534,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4488344212477386,
+          "y" : 1.8357164731969533
+        },
+        "rotation" : {
+          "radians" : -1.745691473047995
+        }
+      },
+      "curvature" : 0.7724972819405648
+    }, {
+      "time" : 2.486114901209423,
+      "velocity" : 1.8780369284715344,
+      "acceleration" : 3.9999999999999947,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4331791216126497,
+          "y" : 1.7189331963567618
+        },
+        "rotation" : {
+          "radians" : -1.6662825767260925
+        }
+      },
+      "curvature" : 0.5761233113953619
+    }, {
+      "time" : 2.546692489588632,
+      "velocity" : 2.1203472819883706,
+      "acceleration" : 3.999999999999993,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4253702445836325,
+          "y" : 1.5980789786207197
+        },
+        "rotation" : {
+          "radians" : -1.608229977974804
+        }
+      },
+      "curvature" : 0.3843159112832851
+    }, {
+      "time" : 2.602172291072171,
+      "velocity" : 2.3422664879225272,
+      "acceleration" : 3.9999999999999893,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4231985572659944,
+          "y" : 1.4743055659818047
+        },
+        "rotation" : {
+          "radians" : -1.5723274201937065
+        }
+      },
+      "curvature" : 0.19618933375882694
+    }, {
+      "time" : 2.6534386588593746,
+      "velocity" : 2.54733195907134,
+      "acceleration" : 3.220008756110653,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4240437746047974,
+          "y" : 1.3489724397659302
+        },
+        "rotation" : {
+          "radians" : -1.5598679357771585
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 2.7013005707324047,
+      "velocity" : 2.7014477343866927,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4258348567705248,
+          "y" : 1.2233768946074477
+        },
+        "rotation" : {
+          "radians" : -1.550010895685447
+        }
+      },
+      "curvature" : 0.15261911450126764
+    }, {
+      "time" : 2.749505539118783,
+      "velocity" : 2.508627860841179,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4299988476074077,
+          "y" : 1.09787018620068
+        },
+        "rotation" : {
+          "radians" : -1.5225811069171706
+        }
+      },
+      "curvature" : 0.28021498367515013
+    }, {
+      "time" : 2.801724619491527,
+      "velocity" : 2.2997515393502033,
+      "acceleration" : -3.6618849413513943,
+      "pose" : {
+        "translation" : {
+          "x" : 1.438542295689782,
+          "y" : 0.972616643926834
+        },
+        "rotation" : {
+          "radians" : -1.4806280826538225
+        }
+      },
+      "curvature" : 0.3842039868268945
+    }, {
+      "time" : 2.858949646384186,
+      "velocity" : 2.090200075103547,
+      "acceleration" : -1.9233425253606922,
+      "pose" : {
+        "translation" : {
+          "x" : 1.45309450732384,
+          "y" : 0.8478549124047277
+        },
+        "rotation" : {
+          "radians" : -1.4270400417472306
+        }
+      },
+      "curvature" : 0.465101762848264
+    }, {
+      "time" : 2.920938902993327,
+      "velocity" : 1.9709735017516907,
+      "acceleration" : -0.9820932714498863,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4749322121688557,
+          "y" : 0.7238891070967739
+        },
+        "rotation" : {
+          "radians" : -1.3645961892537415
+        }
+      },
+      "curvature" : 0.5230727944268867
+    }, {
+      "time" : 2.9861480086005843,
+      "velocity" : 1.906932077897538,
+      "acceleration" : -0.5051437890621265,
+      "pose" : {
+        "translation" : {
+          "x" : 1.505004228858411,
+          "y" : 0.6010799699149629
+        },
+        "rotation" : {
+          "radians" : -1.2959588931506363
+        }
+      },
+      "curvature" : 0.5587959564566138
+    }, {
+      "time" : 3.019621045984329,
+      "velocity" : 1.890023380962095,
+      "acceleration" : -0.2575060162536636,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5233410168520205,
+          "y" : 0.5402351909635741
+        },
+        "rotation" : {
+          "radians" : -1.2601011467472856
+        }
+      },
+      "curvature" : 0.5688389814615953
+    }, {
+      "time" : 3.0534660636487803,
+      "velocity" : 1.8813080852932873,
+      "acceleration" : -0.046118696833170436,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5439561306216198,
+          "y" : 0.4798360248268452
+        },
+        "rotation" : {
+          "radians" : -1.2236026771430364
+        }
+      },
+      "curvature" : 0.5741215645431921
+    }, {
+      "time" : 3.0875714219887946,
+      "velocity" : 1.8797351906116175,
+      "acceleration" : 0.13685239477635067,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5668869314298823,
+          "y" : 0.41993955191350785
+        },
+        "rotation" : {
+          "radians" : -1.1867261461606144
+        }
+      },
+      "curvature" : 0.575082775007997
+    }, {
+      "time" : 3.121837246466734,
+      "velocity" : 1.8844245507504096,
+      "acceleration" : 0.29657244735757804,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5921549109043553,
+          "y" : 0.3606047334615141
+        },
+        "rotation" : {
+          "radians" : -1.1497095266580952
+        }
+      },
+      "curvature" : 0.5722241677945603
+    }, {
+      "time" : 3.15617469824573,
+      "velocity" : 1.8946080928605293,
+      "acceleration" : 0.43641554195126636,
+      "pose" : {
+        "translation" : {
+          "x" : 1.619766461838124,
+          "y" : 0.30189213515072333
+        },
+        "rotation" : {
+          "radians" : -1.1127638757054572
+        }
+      },
+      "curvature" : 0.566089275718927
+    }, {
+      "time" : 3.1905053847904026,
+      "velocity" : 1.9095905380344815,
+      "acceleration" : 0.5585224224331813,
+      "pose" : {
+        "translation" : {
+          "x" : 1.649713648990474,
+          "y" : 0.24386365071558913
+        },
+        "rotation" : {
+          "radians" : -1.076071959756705
+        }
+      },
+      "curvature" : 0.5572411700339329
+    }, {
+      "time" : 3.224760834869715,
+      "velocity" : 1.928722974994318,
+      "acceleration" : 0.6641677388812759,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6819749798875545,
+          "y" : 0.18658222555784643
+        },
+        "rotation" : {
+          "radians" : -1.039787779550021
+        }
+      },
+      "curvature" : 0.5462406244811652
+    }, {
+      "time" : 3.258881991221138,
+      "velocity" : 1.9513851462562573,
+      "acceleration" : 0.7540110868217726,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7165161756230418,
+          "y" : 0.13011158035919834
+        },
+        "rotation" : {
+          "radians" : -1.0040369547627968
+        }
+      },
+      "curvature" : 0.5336268996944579
+    }, {
+      "time" : 3.2928186950481497,
+      "velocity" : 1.976973797192011,
+      "acceleration" : 0.8282759740654535,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7532909416588032,
+          "y" : 0.0745159346940032
+        },
+        "rotation" : {
+          "radians" : -0.9689178606738195
+        }
+      },
+      "curvature" : 0.5199024657633008
+    }, {
+      "time" : 3.3265291512596264,
+      "velocity" : 2.004895358146763,
+      "acceleration" : 0.8868837310332817,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7922417386255596,
+          "y" : 0.01985973064196145
+        },
+        "rotation" : {
+          "radians" : -0.9345033658181425
+        }
+      },
+      "curvature" : 0.5055222588408043
+    }, {
+      "time" : 3.3599793731404124,
+      "velocity" : 2.0345618157322853,
+      "acceleration" : 0.9295593121848896,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8333005531235487,
+          "y" : -0.033792643599197314
+        },
+        "rotation" : {
+          "radians" : -0.9008430006823431
+        }
+      },
+      "curvature" : 0.4908874449734679
+    }, {
+      "time" : 3.3931426109904206,
+      "velocity" : 2.065389012297963,
+      "acceleration" : 0.9559197746209286,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8763896685231884,
+          "y" : -0.08637713010102743
+        },
+        "rotation" : {
+          "radians" : -0.8679653892038623
+        }
+      },
+      "curvature" : 0.4763432100815362
+    }, {
+      "time" : 3.425998772114722,
+      "velocity" : 2.0967968664348136,
+      "acceleration" : 0.96555227037989,
+      "pose" : {
+        "translation" : {
+          "x" : 1.921422435765741,
+          "y" : -0.1378302775916802
+        },
+        "rotation" : {
+          "radians" : -0.8358807916469844
+        }
+      },
+      "curvature" : 0.46217982712431704
+    }, {
+      "time" : 3.4585338402322123,
+      "velocity" : 2.1282111753226207,
+      "acceleration" : 0.9580857106502751,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9683040441639754,
+          "y" : -0.18808951784321692
+        },
+        "rotation" : {
+          "radians" : -0.8045836333638371
+        }
+      },
+      "curvature" : 0.44863614984773975
+    }, {
+      "time" : 3.4907393016272956,
+      "velocity" : 2.159066767690149,
+      "acceleration" : 0.9332583224551695,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0169322922028305,
+          "y" : -0.2370934420589219
+        },
+        "rotation" : {
+          "radians" : -0.774054923708963
+        }
+      },
+      "curvature" : 0.43590470633971296
+    }, {
+      "time" : 3.5226115837946828,
+      "velocity" : 2.1888118402785026,
+      "acceleration" : 0.8909818249589498,
+      "pose" : {
+        "translation" : {
+          "x" : 2.067198358340079,
+          "y" : -0.28478207726061555
+        },
+        "rotation" : {
+          "radians" : -0.744264498898693
+        }
+      },
+      "curvature" : 0.42413766870248887
+    }, {
+      "time" : 3.554151510376562,
+      "velocity" : 2.2169133416234965,
+      "acceleration" : 0.831401748795369,
+      "pose" : {
+        "translation" : {
+          "x" : 2.11898757180699,
+          "y" : -0.33109716267596734
+        },
+        "rotation" : {
+          "radians" : -0.7151730492255899
+        }
+      },
+      "curvature" : 0.41345311620209624
+    }, {
+      "time" : 3.5853637741835547,
+      "velocity" : 2.2428632723364927,
+      "acceleration" : 0.7549524443057307,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1721801834089938,
+          "y" : -0.3759824261258089
+        },
+        "rotation" : {
+          "radians" : -0.6867339133510525
+        }
+      },
+      "curvature" : 0.40394115698677635
+    }, {
+      "time" : 3.6162564282292005,
+      "velocity" : 2.266185757019344,
+      "acceleration" : 0.6624045708844728,
+      "pose" : {
+        "translation" : {
+          "x" : 2.226652136326343,
+          "y" : -0.41938386041144704
+        },
+        "rotation" : {
+          "radians" : -0.6588946399888784
+        }
+      },
+      "curvature" : 0.3956696075854153
+    }, {
+      "time" : 3.646840393102844,
+      "velocity" : 2.286444715147416,
+      "acceleration" : 0.5549023669290096,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2822758369147778,
+          "y" : -0.4612499997019768
+        },
+        "rotation" : {
+          "radians" : -0.6315983304138486
+        }
+      },
+      "curvature" : 0.38868903913170505
+    }, {
+      "time" : 3.677128977700188,
+      "velocity" : 2.3032519224314116,
+      "acceleration" : 0.43398782613727804,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3389209255061876,
+          "y" : -0.5015321959215944
+        },
+        "rotation" : {
+          "radians" : -0.6047847845266212
+        }
+      },
+      "curvature" : 0.38303708086136296
+    }, {
+      "time" : 3.7071374093201945,
+      "velocity" : 2.3162752164359675,
+      "acceleration" : 0.301609110666151,
+      "pose" : {
+        "translation" : {
+          "x" : 2.396455047209276,
+          "y" : -0.5401848951369104
+        },
+        "rotation" : {
+          "radians" : -0.5783914794951374
+        }
+      },
+      "curvature" : 0.37874192561228726
+    }, {
+      "time" : 3.736882368388014,
+      "velocity" : 2.3252465670872136,
+      "acceleration" : 0.16011115647032365,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4547446227102228,
+          "y" : -0.5771659139442626
+        },
+        "rotation" : {
+          "radians" : -0.5523544141104476
+        }
+      },
+      "curvature" : 0.37582501188792183
+    }, {
+      "time" : 3.766381522526342,
+      "velocity" : 2.3299697107711976,
+      "acceleration" : 0.012207476034279642,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5136556190733472,
+          "y" : -0.6124367158570294
+        },
+        "rotation" : {
+          "radians" : -0.5266088547091091
+        }
+      },
+      "curvature" : 0.37430286645269983
+    }, {
+      "time" : 3.7956530543152014,
+      "velocity" : 2.330327042293997,
+      "acceleration" : -0.13906640622183863,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5730543205417726,
+          "y" : -0.6459626876929423
+        },
+        "rotation" : {
+          "radians" : -0.5010900204761772
+        }
+      },
+      "curvature" : 0.37418808431592226
+    }, {
+      "time" : 3.8247151767948284,
+      "velocity" : 2.326285477363576,
+      "acceleration" : -0.29041573965154777,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6328080993380887,
+          "y" : -0.6777134159613993
+        },
+        "rotation" : {
+          "radians" : -0.47573374766730814
+        }
+      },
+      "curvature" : 0.37548940279405946
+    }, {
+      "time" : 3.8535856305237206,
+      "velocity" : 2.317901043189824,
+      "acceleration" : -0.4383615615686928,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6927861864650144,
+          "y" : -0.7076629632507778
+        },
+        "rotation" : {
+          "radians" : -0.45047717414862004
+        }
+      },
+      "curvature" : 0.37821079612304137
+    }, {
+      "time" : 3.882281155755939,
+      "velocity" : 2.305322027938995,
+      "acceleration" : -0.5793221339027272,
+      "pose" : {
+        "translation" : {
+          "x" : 2.752860442506062,
+          "y" : -0.7357901446157484
+        },
+        "rotation" : {
+          "radians" : -0.42525948787674483
+        }
+      },
+      "curvature" : 0.3823494796069915
+    }, {
+      "time" : 3.91081693300839,
+      "velocity" : 2.288790620568532,
+      "acceleration" : -0.7096933712661672,
+      "pose" : {
+        "translation" : {
+          "x" : 2.8129061284262007,
+          "y" : -0.7620788039645863
+        },
+        "rotation" : {
+          "radians" : -0.40002278560350163
+        }
+      },
+      "curvature" : 0.38789267058227345
+    }, {
+      "time" : 3.9392059849192336,
+      "velocity" : 2.2686430986108754,
+      "acceleration" : -0.8749184265202878,
+      "pose" : {
+        "translation" : {
+          "x" : 2.872802676372519,
+          "y" : -0.786518090446485
+        },
+        "rotation" : {
+          "radians" : -0.37471309109137346
+        }
+      },
+      "curvature" : 0.394812911527834
+    }, {
+      "time" : 3.9955941671938477,
+      "velocity" : 2.219308038900831,
+      "acceleration" : -1.0288336535097118,
+      "pose" : {
+        "translation" : {
+          "x" : 2.991691567646626,
+          "y" : -0.8298333259347146
+        },
+        "rotation" : {
+          "radians" : -0.3236861024053773
+        }
+      },
+      "curvature" : 0.4125613419519297
+    }, {
+      "time" : 4.051459900853216,
+      "velocity" : 2.161831492034062,
+      "acceleration" : -1.0796480258443715,
+      "pose" : {
+        "translation" : {
+          "x" : 3.108675287572691,
+          "y" : -0.865765651810257
+        },
+        "rotation" : {
+          "radians" : -0.2718791063461188
+        }
+      },
+      "curvature" : 0.43479047913732005
+    }, {
+      "time" : 4.10675359376877,
+      "velocity" : 2.10213376563614,
+      "acceleration" : -0.9994511460458044,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2230180775513873,
+          "y" : -0.8944474414456636
+        },
+        "rotation" : {
+          "radians" : -0.21916678625150354
+        }
+      },
+      "curvature" : 0.4598360409407436
+    }, {
+      "time" : 4.161311871060626,
+      "velocity" : 2.0476054328705096,
+      "acceleration" : -0.749178067288782,
+      "pose" : {
+        "translation" : {
+          "x" : 3.334124914760981,
+          "y" : -0.9161224852502343
+        },
+        "rotation" : {
+          "radians" : -0.16568957342253765
+        }
+      },
+      "curvature" : 0.48465328108684036
+    }, {
+      "time" : 4.214818370708727,
+      "velocity" : 2.007519536876757,
+      "acceleration" : -1.3699445774534182,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4415661777785544,
+          "y" : -0.9311548350640351
+        },
+        "rotation" : {
+          "radians" : -0.11197409892743718
+        }
+      },
+      "curvature" : 0.5042015108247184
+    }, {
+      "time" : 4.2675300331068176,
+      "velocity" : 1.9353074808059383,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5451023122012337,
+          "y" : -0.9400376485519146
+        },
+        "rotation" : {
+          "radians" : -0.059075560161144215
+        }
+      },
+      "curvature" : 0.5109123503903398
+    }, {
+      "time" : 4.322105290852558,
+      "velocity" : 1.7170064498229765,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6447084962674126,
+          "y" : -0.943402033597522
+        },
+        "rotation" : {
+          "radians" : -0.008719023872416791
+        }
+      },
+      "curvature" : 0.4946090622112777
+    }, {
+      "time" : 4.382159659747878,
+      "velocity" : 1.4767889742416929,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7405993064779786,
+          "y" : -0.9420258926973233
+        },
+        "rotation" : {
+          "radians" : 0.03660524430653648
+        }
+      },
+      "curvature" : 0.4434197350542565
+    }, {
+      "time" : 4.451511716406289,
+      "velocity" : 1.1993807476080485,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8332533832175386,
+          "y" : -0.9368427673546194
+        },
+        "rotation" : {
+          "radians" : 0.07365896092598179
+        }
+      },
+      "curvature" : 0.3462339716324929
+    }, {
+      "time" : 4.540069304785332,
+      "velocity" : 0.8451503940918763,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9234380963756443,
+          "y" : -0.9289506824735625
+        },
+        "rotation" : {
+          "radians" : 0.09866354717797672
+        }
+      },
+      "curvature" : 0.19694019866370868
+    }, {
+      "time" : 4.751356903308301,
+      "velocity" : 0.0,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 4.012234210968018,
+          "y" : -0.9196209907531738
+        },
+        "rotation" : {
+          "radians" : 0.10776957043787032
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.8985185623168945, -6.7758471965789795, 0.0 ],
+      "y" : [ 2.9429874420166016, -0.26568102836608887, 0.0 ]
+    }, {
+      "x" : [ 1.4240437746047974, 0.04392707347869873, 0.0 ],
+      "y" : [ 1.3489724397659302, -4.019377112388611, 0.0 ]
+    }, {
+      "x" : [ 4.012234210968018, 2.833451271057129, 0.0 ],
+      "y" : [ -0.9196209907531738, 0.30654752254486084, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.2617993877991494
+      }
+    }, {
+      "time" : 2.6499999407678843,
+      "rotation" : {
+        "radians" : 0.2617993877991494
+      }
+    }, {
+      "time" : 4.659999895840883,
+      "rotation" : {
+        "radians" : 0.2617993877991494
+      }
+    } ],
+    "reversed" : false,
+    "color" : 52.47058,
+    "closed" : true,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@t 0.89\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 0.89,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "REVERSE" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "EJECT" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.012234210968018,
+          "y" : -0.9196209907531738
+        },
+        "rotation" : {
+          "radians" : 0.030602586025410137
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.18393922011618816,
+      "velocity" : 0.7357568804647526,
+      "acceleration" : 3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 4.079868928493841,
+          "y" : -0.9175222116396733
+        },
+        "rotation" : {
+          "radians" : 0.03184996886547707
+        }
+      },
+      "curvature" : 0.03638401924476792
+    }, {
+      "time" : 0.26009263225753565,
+      "velocity" : 1.0403705290301426,
+      "acceleration" : 3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 4.1474601409882865,
+          "y" : -0.9152593300483591
+        },
+        "rotation" : {
+          "radians" : 0.035461574602982625
+        }
+      },
+      "curvature" : 0.06998452771616054
+    }, {
+      "time" : 0.3184715235418354,
+      "velocity" : 1.2738860941673413,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 4.214962774536021,
+          "y" : -0.9126807022060877
+        },
+        "rotation" : {
+          "radians" : 0.04125182940160068
+        }
+      },
+      "curvature" : 0.1010514081151579
+    }, {
+      "time" : 0.36761655293009865,
+      "velocity" : 1.4704662117203942,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 4.2823301801504385,
+          "y" : -0.9096479069208385
+        },
+        "rotation" : {
+          "radians" : 0.04904809314793145
+        }
+      },
+      "curvature" : 0.12981170585502438
+    }, {
+      "time" : 0.4108314858481353,
+      "velocity" : 1.643325943392541,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 4.349514283856005,
+          "y" : -0.9060353399753465
+        },
+        "rotation" : {
+          "radians" : 0.05868962955532622
+        }
+      },
+      "curvature" : 0.15646782863946646
+    }, {
+      "time" : 0.44980816479061647,
+      "velocity" : 1.7992326591624657,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.416465736770604,
+          "y" : -0.9017298085207326
+        },
+        "rotation" : {
+          "radians" : 0.07002652572535734
+        }
+      },
+      "curvature" : 0.1811979163878778
+    }, {
+      "time" : 0.4855501313208,
+      "velocity" : 1.9422005252831998,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.48313406518789,
+          "y" : -0.8966301254701357
+        },
+        "rotation" : {
+          "radians" : 0.08291864609124491
+        }
+      },
+      "curvature" : 0.20415755775322134
+    }, {
+      "time" : 0.5187103847104131,
+      "velocity" : 2.0748415388416523,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 4.549467820659629,
+          "y" : -0.8906467038923438
+        },
+        "rotation" : {
+          "radians" : 0.0972346733352272
+        }
+      },
+      "curvature" : 0.22548228467606032
+    }, {
+      "time" : 0.5497420218631486,
+      "velocity" : 2.1989680874525943,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 4.615414730078044,
+          "y" : -0.8837011514054263
+        },
+        "rotation" : {
+          "radians" : 0.1128512659439996
+        }
+      },
+      "curvature" : 0.24529045745559744
+    }, {
+      "time" : 0.5789746947138208,
+      "velocity" : 2.315898778855283,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 4.680921845758164,
+          "y" : -0.8757258645703647
+        },
+        "rotation" : {
+          "radians" : 0.12965234596402142
+        }
+      },
+      "curvature" : 0.263686278415705
+    }, {
+      "time" : 0.6066571501940509,
+      "velocity" : 2.4266286007762035,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.745935695520172,
+          "y" : -0.8666636232846845
+        },
+        "rotation" : {
+          "radians" : 0.14752851931001348
+        }
+      },
+      "curvature" : 0.2807627581933577
+    }, {
+      "time" : 0.6329825891472886,
+      "velocity" : 2.531930356589154,
+      "acceleration" : 0.9017846067562265,
+      "pose" : {
+        "translation" : {
+          "x" : 4.8104024327717525,
+          "y" : -0.8564671851760863
+        },
+        "rotation" : {
+          "radians" : 0.1663766232708782
+        }
+      },
+      "curvature" : 0.29660451731621684
+    }, {
+      "time" : 0.6584873043239601,
+      "velocity" : 2.5549301161351785,
+      "acceleration" : -2.1207024597033652,
+      "pose" : {
+        "translation" : {
+          "x" : 4.87426798659043,
+          "y" : -0.8450988799960781
+        },
+        "rotation" : {
+          "radians" : 0.18609939066967865
+        }
+      },
+      "curvature" : 0.3112903462074792
+    }, {
+      "time" : 0.683981890618659,
+      "velocity" : 2.500863684270891,
+      "acceleration" : -1.823821978528963,
+      "pose" : {
+        "translation" : {
+          "x" : 4.937478211805924,
+          "y" : -0.8325302040136062
+        },
+        "rotation" : {
+          "radians" : 0.20660521679714633
+        }
+      },
+      "curvature" : 0.3248954755335266
+    }, {
+      "time" : 0.7098179589134984,
+      "velocity" : 2.4537432950759874,
+      "acceleration" : -1.5828512441983833,
+      "pose" : {
+        "translation" : {
+          "x" : 4.999979039082493,
+          "y" : -0.8187414144086866
+        },
+        "rotation" : {
+          "radians" : 0.22780801329582814
+        }
+      },
+      "curvature" : 0.3374935302608176
+    }, {
+      "time" : 0.7359324293185369,
+      "velocity" : 2.4124079731037904,
+      "acceleration" : -1.3044976586946302,
+      "pose" : {
+        "translation" : {
+          "x" : 5.0617166250012815,
+          "y" : -0.8037211236660369
+        },
+        "rotation" : {
+          "radians" : 0.24962713231241265
+        }
+      },
+      "curvature" : 0.3491581576802025
+    }, {
+      "time" : 0.7887463969414447,
+      "velocity" : 2.343512275993333,
+      "acceleration" : -1.0334589786307198,
+      "pose" : {
+        "translation" : {
+          "x" : 5.182688729168589,
+          "y" : -0.7699798315917139
+        },
+        "rotation" : {
+          "radians" : 0.29481885303544775
+        }
+      },
+      "curvature" : 0.36998936253987214
+    }, {
+      "time" : 0.8420624190091038,
+      "velocity" : 2.288412354282637,
+      "acceleration" : -0.8513997934004414,
+      "pose" : {
+        "translation" : {
+          "x" : 5.299973998423866,
+          "y" : -0.7313669207204043
+        },
+        "rotation" : {
+          "radians" : 0.3416439837677253
+        }
+      },
+      "curvature" : 0.3880209156918146
+    }, {
+      "time" : 0.8955555645929367,
+      "velocity" : 2.242868301184222,
+      "acceleration" : -0.736658519915954,
+      "pose" : {
+        "translation" : {
+          "x" : 5.413165126823472,
+          "y" : -0.6880507100495148
+        },
+        "rotation" : {
+          "radians" : 0.389654433979741
+        }
+      },
+      "curvature" : 0.403939345595272
+    }, {
+      "time" : 0.9489629734818855,
+      "velocity" : 2.203525278399543,
+      "acceleration" : -0.6767969498415867,
+      "pose" : {
+        "translation" : {
+          "x" : 5.5218728309992,
+          "y" : -0.6402942652402999
+        },
+        "rotation" : {
+          "radians" : 0.43849160494770656
+        }
+      },
+      "curvature" : 0.41849245218579234
+    }, {
+      "time" : 1.0020763940584052,
+      "velocity" : 2.167578277357701,
+      "acceleration" : -0.6641591172113208,
+      "pose" : {
+        "translation" : {
+          "x" : 5.625730652793365,
+          "y" : -0.5884424192140703
+        },
+        "rotation" : {
+          "radians" : 0.48788654495705386
+        }
+      },
+      "curvature" : 0.4324880614534242
+    }, {
+      "time" : 1.0547362906352467,
+      "velocity" : 2.1326037269347866,
+      "acceleration" : -0.693255970599057,
+      "pose" : {
+        "translation" : {
+          "x" : 5.724399761893892,
+          "y" : -0.5329087927484011
+        },
+        "rotation" : {
+          "radians" : 0.5376585244703848
+        }
+      },
+      "curvature" : 0.4467899297328142
+    }, {
+      "time" : 1.1068266505846867,
+      "velocity" : 2.0964917738891833,
+      "acceleration" : -0.7590255374673471,
+      "pose" : {
+        "translation" : {
+          "x" : 5.817573758469408,
+          "y" : -0.4741628150733401
+        },
+        "rotation" : {
+          "radians" : 0.5877118587042041
+        }
+      },
+      "curvature" : 0.4623143546066941
+    }, {
+      "time" : 1.1582699222585076,
+      "velocity" : 2.0574450169578826,
+      "acceleration" : -0.85555708396171,
+      "pose" : {
+        "translation" : {
+          "x" : 5.904983475804329,
+          "y" : -0.4127167444676161
+        },
+        "rotation" : {
+          "radians" : 0.638030997754264
+        }
+      },
+      "curvature" : 0.4800287263774246
+    }, {
+      "time" : 1.20902166214638,
+      "velocity" : 2.014024006373431,
+      "acceleration" : -0.9751306653340864,
+      "pose" : {
+        "translation" : {
+          "x" : 5.986401782933953,
+          "y" : -0.3491126888548468
+        },
+        "rotation" : {
+          "radians" : 0.6886738736416058
+        }
+      },
+      "curvature" : 0.500950042603538
+    }, {
+      "time" : 1.259064520666594,
+      "velocity" : 1.9652256804493953,
+      "acceleration" : -1.1075634112905204,
+      "pose" : {
+        "translation" : {
+          "x" : 6.061648387279547,
+          "y" : -0.2839096263997476
+        },
+        "rotation" : {
+          "radians" : 0.7397632167792167
+        }
+      },
+      "curvature" : 0.5261369978659279
+    }, {
+      "time" : 1.3084011828915338,
+      "velocity" : 1.9105821985338527,
+      "acceleration" : -1.239915254926958,
+      "pose" : {
+        "translation" : {
+          "x" : 6.130594637283437,
+          "y" : -0.2176704261043394
+        },
+        "rotation" : {
+          "radians" : 0.7914750099875708
+        }
+      },
+      "curvature" : 0.5566628639559956
+    }, {
+      "time" : 1.3570458238963947,
+      "velocity" : 1.8502669660814803,
+      "acceleration" : -1.3565877307387766,
+      "pose" : {
+        "translation" : {
+          "x" : 6.193168325044098,
+          "y" : -0.1509488684041571
+        },
+        "rotation" : {
+          "radians" : 0.8440224046461846
+        }
+      },
+      "curvature" : 0.5935467253109423
+    }, {
+      "time" : 1.4050135519869393,
+      "velocity" : 1.7851945346824336,
+      "acceleration" : -1.439730441538962,
+      "pose" : {
+        "translation" : {
+          "x" : 6.249358488951245,
+          "y" : -0.0842766657644578
+        },
+        "rotation" : {
+          "radians" : 0.8976322459176475
+        }
+      },
+      "curvature" : 0.6376063101064825
+    }, {
+      "time" : 1.4523072062995137,
+      "velocity" : 1.7171044208769999,
+      "acceleration" : -1.4696274540546912,
+      "pose" : {
+        "translation" : {
+          "x" : 6.2992202163209186,
+          "y" : -0.018150483276428986
+        },
+        "rotation" : {
+          "radians" : 0.9525098498997762
+        }
+      },
+      "curvature" : 0.6891762313763988
+    }, {
+      "time" : 1.4989007546283921,
+      "velocity" : 1.6486292630710562,
+      "acceleration" : -1.4243277798746545,
+      "pose" : {
+        "translation" : {
+          "x" : 6.3428794460305795,
+          "y" : 0.04698104074660314
+        },
+        "rotation" : {
+          "radians" : 1.0087860125729582
+        }
+      },
+      "curvature" : 0.7476144640522491
+    }, {
+      "time" : 1.544718402885982,
+      "velocity" : 1.5833699138492454,
+      "acceleration" : -1.277051637682657,
+      "pose" : {
+        "translation" : {
+          "x" : 6.380537771154195,
+          "y" : 0.11073027417296544
+        },
+        "rotation" : {
+          "radians" : 1.0664389649669272
+        }
+      },
+      "curvature" : 0.8105110247651124
+    }, {
+      "time" : 1.589608352044505,
+      "velocity" : 1.5260431307608624,
+      "acceleration" : -0.9882350321042662,
+      "pose" : {
+        "translation" : {
+          "x" : 6.41247724159733,
+          "y" : 0.17278357045642956
+        },
+        "rotation" : {
+          "radians" : 1.1251844168307794
+        }
+      },
+      "curvature" : 0.8725495261033641
+    }, {
+      "time" : 1.6333097898626403,
+      "velocity" : 1.482855838955655,
+      "acceleration" : -0.4841929472411742,
+      "pose" : {
+        "translation" : {
+          "x" : 6.4390651667322345,
+          "y" : 0.2329142480400037
+        },
+        "rotation" : {
+          "radians" : 1.1843313194749898
+        }
+      },
+      "curvature" : 0.9241146185221707
+    }, {
+      "time" : 1.6754107043468742,
+      "velocity" : 1.4624708730899851,
+      "acceleration" : -3.7808476405994242,
+      "pose" : {
+        "translation" : {
+          "x" : 6.460758918032937,
+          "y" : 0.2909955697597244
+        },
+        "rotation" : {
+          "radians" : 1.2426127877010575
+        }
+      },
+      "curvature" : 0.9500561047849169
+    }, {
+      "time" : 1.7178366346797787,
+      "velocity" : 1.3020648944905882,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.478110731710331,
+          "y" : 0.3470137222484482
+        },
+        "rotation" : {
+          "radians" : 1.2980233032835509
+        }
+      },
+      "curvature" : 0.9295171145481738
+    }, {
+      "time" : 1.7639291544880524,
+      "velocity" : 1.117694815257493,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 6.491772511347264,
+          "y" : 0.40108079533964336
+        },
+        "rotation" : {
+          "radians" : 1.3477231257633573
+        }
+      },
+      "curvature" : 0.838208547283802
+    }, {
+      "time" : 1.8167467720854111,
+      "velocity" : 0.9064243448680579,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.502500630533632,
+          "y" : 0.4534477614711818
+        },
+        "rotation" : {
+          "radians" : 1.3880989582767027
+        }
+      },
+      "curvature" : 0.6544921319595709
+    }, {
+      "time" : 2.0433528583024256,
+      "velocity" : 0.0,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.51873254776001,
+          "y" : 0.5548575520515442
+        },
+        "rotation" : {
+          "radians" : 1.4247813178399886
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ],
+      "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ]
+    }, {
+      "x" : [ 6.51873254776001, 0.23627901077270508, 0.0 ],
+      "y" : [ 0.5548575520515442, 1.6066665649414062, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : -0.13962634015954636
+      }
+    }, {
+      "time" : 1.9499999564141035,
+      "rotation" : {
+        "radians" : -0.1526493858119171
+      }
+    } ],
+    "reversed" : false,
+    "color" : 286.3529,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "sleep 2000",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "sleep",
+        "args" : [ "2000" ],
+        "argTypes" : [ "long" ],
+        "reflection" : false
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 6.51873254776001,
+          "y" : 0.5548575520515442
+        },
+        "rotation" : {
+          "radians" : 1.4550369939781984
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.1788531666911449,
+      "velocity" : 0.7154126667645795,
+      "acceleration" : 3.999999999999994,
+      "pose" : {
+        "translation" : {
+          "x" : 6.526029236446348,
+          "y" : 0.6184169998749507
+        },
+        "rotation" : {
+          "radians" : 1.4593465391163816
+        }
+      },
+      "curvature" : 0.13050101301586398
+    }, {
+      "time" : 0.25321186756390435,
+      "velocity" : 1.012847470255617,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.532831722088758,
+          "y" : 0.6823114984076142
+        },
+        "rotation" : {
+          "radians" : 1.471236108776517
+        }
+      },
+      "curvature" : 0.23504966165445965
+    }, {
+      "time" : 0.3106349893777712,
+      "velocity" : 1.2425399575110843,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.538718402694414,
+          "y" : 0.746799069553143
+        },
+        "rotation" : {
+          "radians" : 1.4891403961437595
+        }
+      },
+      "curvature" : 0.31358925447041286
+    }, {
+      "time" : 0.3594524552162631,
+      "velocity" : 1.437809820865052,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 6.543342575678253,
+          "y" : 0.8120593882322282
+        },
+        "rotation" : {
+          "radians" : 1.511568465726473
+        }
+      },
+      "curvature" : 0.3681705103657191
+    }, {
+      "time" : 0.4028800665262472,
+      "velocity" : 1.611520266104988,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 6.546427705950194,
+          "y" : 0.8782000352682129
+        },
+        "rotation" : {
+          "radians" : 1.5371749251346154
+        }
+      },
+      "curvature" : 0.4020595428820239
+    }, {
+      "time" : 0.44254982612556115,
+      "velocity" : 1.7701993045022437,
+      "acceleration" : 3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 6.547762694002358,
+          "y" : 0.9452627502726614
+        },
+        "rotation" : {
+          "radians" : 1.5647989424802173
+        }
+      },
+      "curvature" : 0.41904605440094195
+    }, {
+      "time" : 0.4794110889330761,
+      "velocity" : 1.9176443557323033,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.547197143996286,
+          "y" : 1.0132296845309288
+        },
+        "rotation" : {
+          "radians" : 1.5934786052963947
+        }
+      },
+      "curvature" : 0.42293973727499673
+    }, {
+      "time" : 0.5140610781999098,
+      "velocity" : 2.056244312799638,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.544636631850153,
+          "y" : 1.08202965388773
+        },
+        "rotation" : {
+          "radians" : 1.622447172268418
+        }
+      },
+      "curvature" : 0.41723748069828426
+    }, {
+      "time" : 0.5468931637504397,
+      "velocity" : 2.187572655001758,
+      "acceleration" : 3.1633627206418256,
+      "pose" : {
+        "translation" : {
+          "x" : 6.540037973325994,
+          "y" : 1.1515443916327097
+        },
+        "rotation" : {
+          "radians" : 1.651117099798838
+        }
+      },
+      "curvature" : 0.4049376530908519
+    }, {
+      "time" : 0.5783519506192766,
+      "velocity" : 2.287088208619253,
+      "acceleration" : 1.870434307392289,
+      "pose" : {
+        "translation" : {
+          "x" : 6.533404492116915,
+          "y" : 1.2216148013860106
+        },
+        "rotation" : {
+          "radians" : 1.6790569837278395
+        }
+      },
+      "curvature" : 0.38847034737186514
+    }, {
+      "time" : 0.608993635935249,
+      "velocity" : 2.3444014680705667,
+      "acceleration" : 2.165397894604544,
+      "pose" : {
+        "translation" : {
+          "x" : 6.524781287934317,
+          "y" : 1.2920472099838438
+        },
+        "rotation" : {
+          "radians" : 1.7059655876740503
+        }
+      },
+      "curvature" : 0.36970875427049765
+    }, {
+      "time" : 0.6390132720691309,
+      "velocity" : 2.409405924951669,
+      "acceleration" : 2.411756982149806,
+      "pose" : {
+        "translation" : {
+          "x" : 6.514250504595111,
+          "y" : 1.3626196203640575
+        },
+        "rotation" : {
+          "radians" : 1.731646007957078
+        }
+      },
+      "curvature" : 0.3500287810963245
+    }, {
+      "time" : 0.668275787449217,
+      "velocity" : 2.479980000734858,
+      "acceleration" : 2.6202614568139766,
+      "pose" : {
+        "translation" : {
+          "x" : 6.501926598108938,
+          "y" : 1.4330879644517065
+        },
+        "rotation" : {
+          "radians" : 1.7559819224996793
+        }
+      },
+      "curvature" : 0.3303903442679772
+    }, {
+      "time" : 0.6966740713701192,
+      "velocity" : 2.554390929532458,
+      "acceleration" : 2.7997101251293675,
+      "pose" : {
+        "translation" : {
+          "x" : 6.487951604765385,
+          "y" : 1.5031923560446216
+        },
+        "rotation" : {
+          "radians" : 1.7789169353574112
+        }
+      },
+      "curvature" : 0.3114217758149773
+    }, {
+      "time" : 0.7241232587242202,
+      "velocity" : 2.6312406972943077,
+      "acceleration" : 1.2935503366228165,
+      "pose" : {
+        "translation" : {
+          "x" : 6.4724904092212086,
+          "y" : 1.572663343698979
+        },
+        "rotation" : {
+          "radians" : 1.8004373323306622
+        }
+      },
+      "curvature" : 0.2934962442471143
+    }, {
+      "time" : 0.7507742479488775,
+      "velocity" : 2.665715093377194,
+      "acceleration" : -3.999999999999994,
+      "pose" : {
+        "translation" : {
+          "x" : 6.455726012587547,
+          "y" : 1.6412281636148691
+        },
+        "rotation" : {
+          "radians" : 1.8205581086255052
+        }
+      },
+      "curvature" : 0.27679481544391576
+    }, {
+      "time" : 0.7774623046076926,
+      "velocity" : 2.5589628667419335,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 6.437854800517144,
+          "y" : 1.7086169925218666
+        },
+        "rotation" : {
+          "radians" : 1.8393118764446346
+        }
+      },
+      "curvature" : 0.2613545325842005
+    }, {
+      "time" : 0.8048451310611667,
+      "velocity" : 2.449431560928037,
+      "acceleration" : -3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 6.419081811291562,
+          "y" : 1.7745692005645992
+        },
+        "rotation" : {
+          "radians" : 1.856740154150982
+        }
+      },
+      "curvature" : 0.24710198818221107
+    }, {
+      "time" : 0.8329039549239173,
+      "velocity" : 2.3371962654770346,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.399616003908406,
+          "y" : 1.8388396041883173
+        },
+        "rotation" : {
+          "radians" : 1.8728865291753276
+        }
+      },
+      "curvature" : 0.23387381346481986
+    }, {
+      "time" : 0.861625693206511,
+      "velocity" : 2.22230931234666,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 6.379665526168537,
+          "y" : 1.901204719024463
+        },
+        "rotation" : {
+          "radians" : 1.8877912376445647
+        }
+      },
+      "curvature" : 0.22142585640379966
+    }, {
+      "time" : 0.921065056241802,
+      "velocity" : 1.9845518602054957,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.339110703361712,
+          "y" : 2.0194711581041815
+        },
+        "rotation" : {
+          "radians" : 1.9139944386552825
+        }
+      },
+      "curvature" : 0.19748286503535473
+    }, {
+      "time" : 0.9834254421957473,
+      "velocity" : 1.7351103163897141,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 6.298886488657445,
+          "y" : 2.1282522362307645
+        },
+        "rotation" : {
+          "radians" : 1.9354586912044225
+        }
+      },
+      "curvature" : 0.17157685598367362
+    }, {
+      "time" : 1.0497222979942022,
+      "velocity" : 1.4699228931958945,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.260146206276204,
+          "y" : 2.2271790434867285
+        },
+        "rotation" : {
+          "radians" : 1.9520508222111153
+        }
+      },
+      "curvature" : 0.13862722514403217
+    }, {
+      "time" : 1.1228723260834714,
+      "velocity" : 1.1773227808388174,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 6.223575937663554,
+          "y" : 2.316830133948315
+        },
+        "rotation" : {
+          "radians" : 1.9634500574230802
+        }
+      },
+      "curvature" : 0.09407105630824328
+    }, {
+      "time" : 1.2119347577273158,
+      "velocity" : 0.8210730542634398,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.189243100281146,
+          "y" : 2.3989316180237097
+        },
+        "rotation" : {
+          "radians" : 1.969501422562912
+        }
+      },
+      "curvature" : 0.0409346875176687
+    }, {
+      "time" : 1.4172030212931759,
+      "velocity" : 0.0,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.156445026397705,
+          "y" : 2.4765572547912598
+        },
+        "rotation" : {
+          "radians" : 1.9710366419843302
+        }
+      },
+      "curvature" : -0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 6.51873254776001, 0.23627471923828125, 0.0 ],
+      "y" : [ 0.5548575520515442, 2.0319605469703674, 0.0 ]
+    }, {
+      "x" : [ 6.156445026397705, -0.519803524017334, 0.0 ],
+      "y" : [ 2.4765572547912598, 1.2286276817321777, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.4704038872114269
+      }
+    }, {
+      "time" : 1.3299999702721834,
+      "rotation" : {
+        "radians" : 0.8760571821323136
+      }
+    } ],
+    "reversed" : false,
+    "color" : 74.117645,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  } ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/autos/blue/movecargotohanger.json b/src/main/deploy/autos/blue/movecargotohanger.json
new file mode 100644
index 00000000..7607216c
--- /dev/null
+++ b/src/main/deploy/autos/blue/movecargotohanger.json
@@ -0,0 +1,3505 @@
+{
+  "autonomousSteps" : [ {
+    "type" : "script",
+    "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "OPEN" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "INTAKE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "ON" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "@t 0.01\nDrive.setAutoRotation 173",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 0.01,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Drive.setAutoRotation",
+        "args" : [ "173" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.6141839027404785,
+          "y" : 1.5894602537155151
+        },
+        "rotation" : {
+          "radians" : 2.389889403743233
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.2489252451296045,
+      "velocity" : 0.995700980518418,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 6.523176953443453,
+          "y" : 1.6735778746319738
+        },
+        "rotation" : {
+          "radians" : 2.4064673708251867
+        }
+      },
+      "curvature" : 0.25911994232595476
+    }, {
+      "time" : 0.3496677436327589,
+      "velocity" : 1.3986709745310357,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 6.432098919045529,
+          "y" : 1.7526409019337734
+        },
+        "rotation" : {
+          "radians" : 2.4514777463969604
+        }
+      },
+      "curvature" : 0.48603924486171285
+    }, {
+      "time" : 0.4240995092316322,
+      "velocity" : 1.6963980369265288,
+      "acceleration" : -2.790576412342658,
+      "pose" : {
+        "translation" : {
+          "x" : 6.340960895459375,
+          "y" : 1.823079613135178
+        },
+        "rotation" : {
+          "radians" : 2.519876340424447
+        }
+      },
+      "curvature" : 0.701310596977904
+    }, {
+      "time" : 0.49210983031570427,
+      "velocity" : 1.5066100391134667,
+      "acceleration" : -1.7711769053800082,
+      "pose" : {
+        "translation" : {
+          "x" : 6.249847903382033,
+          "y" : 1.8827598318457603
+        },
+        "rotation" : {
+          "radians" : 2.607059147103057
+        }
+      },
+      "curvature" : 0.895203949702412
+    }, {
+      "time" : 0.5274607298209384,
+      "velocity" : 1.4439973423253865,
+      "acceleration" : -1.205251380120216,
+      "pose" : {
+        "translation" : {
+          "x" : 6.204343319524796,
+          "y" : 1.9082412143392418
+        },
+        "rotation" : {
+          "radians" : 2.6558796477618074
+        }
+      },
+      "curvature" : 0.9745203573253068
+    }, {
+      "time" : 0.563119080457446,
+      "velocity" : 1.401020066007925,
+      "acceleration" : -0.687574960710818,
+      "pose" : {
+        "translation" : {
+          "x" : 6.15890158834145,
+          "y" : 1.9307801831205325
+        },
+        "rotation" : {
+          "radians" : 2.706939505672123
+        }
+      },
+      "curvature" : 1.0352255751207677
+    }, {
+      "time" : 0.5987086013693859,
+      "velocity" : 1.376549602565181,
+      "acceleration" : -0.17765980611680962,
+      "pose" : {
+        "translation" : {
+          "x" : 6.113546424879203,
+          "y" : 1.9504234287532398
+        },
+        "rotation" : {
+          "radians" : 2.7591326119824173
+        }
+      },
+      "curvature" : 1.0723584365876502
+    }, {
+      "time" : 0.6338600425589757,
+      "velocity" : 1.3703046043387122,
+      "acceleration" : 0.36242756952241223,
+      "pose" : {
+        "translation" : {
+          "x" : 6.068302920743008,
+          "y" : 1.9672693488100776
+        },
+        "rotation" : {
+          "radians" : 2.81126068578529
+        }
+      },
+      "curvature" : 1.0821549980493446
+    }, {
+      "time" : 0.6682116558661673,
+      "velocity" : 1.3827545760588114,
+      "acceleration" : 0.9785292578911144,
+      "pose" : {
+        "translation" : {
+          "x" : 6.023197003472021,
+          "y" : 1.9814617121025577
+        },
+        "rotation" : {
+          "radians" : 2.862095726190118
+        }
+      },
+      "curvature" : 1.0627558269737196
+    }, {
+      "time" : 0.7014108163316237,
+      "velocity" : 1.4152409259116825,
+      "acceleration" : 2.2441271164822805,
+      "pose" : {
+        "translation" : {
+          "x" : 5.978254895916052,
+          "y" : 1.9931833229106815
+        },
+        "rotation" : {
+          "radians" : 2.91045196330826
+        }
+      },
+      "curvature" : 1.0145254489555913
+    }, {
+      "time" : 0.7626515005088358,
+      "velocity" : 1.5526728059056913,
+      "acceleration" : 3.9999999999999925,
+      "pose" : {
+        "translation" : {
+          "x" : 5.888965234160423,
+          "y" : 2.010102666914463
+        },
+        "rotation" : {
+          "radians" : 2.9955886329173094
+        }
+      },
+      "curvature" : 0.8428762374068068
+    }, {
+      "time" : 0.8162079335949602,
+      "velocity" : 1.7668985382501887,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.800629080792987,
+          "y" : 2.0200297651595065
+        },
+        "rotation" : {
+          "radians" : 3.0601867848614575
+        }
+      },
+      "curvature" : 0.6011409133906767
+    }, {
+      "time" : 0.8631605793414101,
+      "velocity" : 1.9547091212359882,
+      "acceleration" : -2.958595604807148,
+      "pose" : {
+        "translation" : {
+          "x" : 5.713411706194165,
+          "y" : 2.025186080179992
+        },
+        "rotation" : {
+          "radians" : 3.1009099300153964
+        }
+      },
+      "curvature" : 0.3280169918222083
+    }, {
+      "time" : 0.9087395705485002,
+      "velocity" : 1.8198593181791478,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 5.627431205854464,
+          "y" : 2.027809408056328
+        },
+        "rotation" : {
+          "radians" : 3.1173842319085643
+        }
+      },
+      "curvature" : 0.05731406395107486
+    }, {
+      "time" : 0.957952684084819,
+      "velocity" : 1.6230068640338726,
+      "acceleration" : -4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 5.5427412004210055,
+          "y" : 2.0299511337652802
+        },
+        "rotation" : {
+          "radians" : 3.1118848820659286
+        }
+      },
+      "curvature" : -0.17922803729629205
+    }, {
+      "time" : 1.0131511813558638,
+      "velocity" : 1.4022128749496938,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.459313535744059,
+          "y" : 2.0332734865301063
+        },
+        "rotation" : {
+          "radians" : 3.0893774981886746
+        }
+      },
+      "curvature" : -0.3450488004527902
+    }, {
+      "time" : 1.077964961333034,
+      "velocity" : 1.1429577550410128,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.377020982923568,
+          "y" : 2.0388467951706843
+        },
+        "rotation" : {
+          "radians" : 3.0578354549476097
+        }
+      },
+      "curvature" : -0.3971872754778253
+    }, {
+      "time" : 1.161849251134293,
+      "velocity" : 0.8074205958359769,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.295619938355685,
+          "y" : 2.0469467434536455
+        },
+        "rotation" : {
+          "radians" : 3.0283983068361184
+        }
+      },
+      "curvature" : -0.2932585838727471
+    }, {
+      "time" : 1.3637044000932872,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.214733123779297,
+          "y" : 2.056851625442505
+        },
+        "rotation" : {
+          "radians" : 3.0150708953014127
+        }
+      },
+      "curvature" : -0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 6.6141839027404785, -1.4558343887329102, 0.0 ],
+      "y" : [ 1.5894602537155151, 1.360889196395874, 0.0 ]
+    }, {
+      "x" : [ 5.214733123779297, -1.292677402496338, 0.0 ],
+      "y" : [ 2.056851625442505, 0.1644301414489746, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.3941697598804894
+      }
+    }, {
+      "time" : 1.269999971613288,
+      "rotation" : {
+        "radians" : 3.0222255927586548
+      }
+    } ],
+    "reversed" : false,
+    "color" : 209.88235,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 10",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.VisionManager.shootBalls",
+        "args" : [ "10" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "Hopper.setEjectOverride true\nDrive.turnToAngle 45",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setEjectOverride",
+        "args" : [ "true" ],
+        "argTypes" : [ "boolean" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Drive.turnToAngle",
+        "args" : [ "45" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.214733123779297,
+          "y" : 2.056851625442505
+        },
+        "rotation" : {
+          "radians" : 0.9530775920796433
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.22164892589636578,
+      "velocity" : 0.8865957035854634,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.272283410420641,
+          "y" : 2.136490201934066
+        },
+        "rotation" : {
+          "radians" : 0.93154333853559
+        }
+      },
+      "curvature" : -0.3354703724748357
+    }, {
+      "time" : 0.32212126730993046,
+      "velocity" : 1.2884850692397223,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 5.339010516181588,
+          "y" : 2.223017249489203
+        },
+        "rotation" : {
+          "radians" : 0.8980578131179813
+        }
+      },
+      "curvature" : -0.24297628723974596
+    }, {
+      "time" : 0.40798632695507736,
+      "velocity" : 1.6319453078203097,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.418256742414087,
+          "y" : 2.320179727881623
+        },
+        "rotation" : {
+          "radians" : 0.8787668506583872
+        }
+      },
+      "curvature" : -0.07195280186555318
+    }, {
+      "time" : 0.4482103474158235,
+      "velocity" : 1.7928413896632942,
+      "acceleration" : 4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 5.462294308261335,
+          "y" : 2.373142516471262
+        },
+        "rotation" : {
+          "radians" : 0.8763509051285007
+        }
+      },
+      "curvature" : -0.0010310805010032027
+    }, {
+      "time" : 0.48701783511066765,
+      "velocity" : 1.9480713404426708,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.508707731962204,
+          "y" : 2.428952641785145
+        },
+        "rotation" : {
+          "radians" : 0.8784798678297706
+        }
+      },
+      "curvature" : 0.057187573384384974
+    }, {
+      "time" : 0.5244749433253648,
+      "velocity" : 2.0978997733014593,
+      "acceleration" : -3.006834100609329,
+      "pose" : {
+        "translation" : {
+          "x" : 5.5569134053257585,
+          "y" : 2.4874170951864016
+        },
+        "rotation" : {
+          "radians" : 0.8846611837531144
+        }
+      },
+      "curvature" : 0.10384088533571874
+    }, {
+      "time" : 0.5628765788352517,
+      "velocity" : 1.9824324261311614,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 5.606271566124633,
+          "y" : 2.5482597037116648
+        },
+        "rotation" : {
+          "radians" : 0.8942836471940969
+        }
+      },
+      "curvature" : 0.13988861038856043
+    }, {
+      "time" : 0.6051667706790917,
+      "velocity" : 1.813271658755801,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.656145019875112,
+          "y" : 2.6111436507878807
+        },
+        "rotation" : {
+          "radians" : 0.9066044762714179
+        }
+      },
+      "curvature" : 0.16515378657020438
+    }, {
+      "time" : 0.6526161179857626,
+      "velocity" : 1.6234742695291178,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.7059578616172075,
+          "y" : 2.6756939969491214
+        },
+        "rotation" : {
+          "radians" : 0.9206836126942823
+        }
+      },
+      "curvature" : 0.17781422207911715
+    }, {
+      "time" : 0.7069026874415512,
+      "velocity" : 1.4063279917059635,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.75525419769474,
+          "y" : 2.7415202005533956
+        },
+        "rotation" : {
+          "radians" : 0.9352834691742239
+        }
+      },
+      "curvature" : 0.1740904256082478
+    }, {
+      "time" : 0.7714879126896856,
+      "velocity" : 1.1479870907134255,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.803756867535412,
+          "y" : 2.8082386384994606
+        },
+        "rotation" : {
+          "radians" : 0.9487507880978892
+        }
+      },
+      "curvature" : 0.14816445891139096
+    }, {
+      "time" : 0.8556329295191124,
+      "velocity" : 0.8114070233957184,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.851426165430894,
+          "y" : 2.8754951269436333
+        },
+        "rotation" : {
+          "radians" : 0.9589069733276255
+        }
+      },
+      "curvature" : 0.09262519823750537
+    }, {
+      "time" : 1.058484685368042,
+      "velocity" : 0.0,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.8985185623168945,
+          "y" : 2.9429874420166016
+        },
+        "rotation" : {
+          "radians" : 0.9629949901604784
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.214733123779297, 0.4449276924133301, 0.0 ],
+      "y" : [ 2.056851625442505, 0.6262435913085938, 0.0 ]
+    }, {
+      "x" : [ 5.8985185623168945, 0.7516036033630371, 0.0 ],
+      "y" : [ 2.9429874420166016, 1.0804316997528076, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.9599310885968813
+      }
+    }, {
+      "time" : 0.9699999783188105,
+      "rotation" : {
+        "radians" : 0.9599310885968813
+      }
+    } ],
+    "reversed" : false,
+    "color" : 148.47061,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@t 1\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 1.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "REVERSE" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "EJECT" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "@t 1.6\nHopper.setHopperState ON\nIntake.setWantedIntakeState INTAKE",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 1.6,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "ON" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "INTAKE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.8985185623168945,
+          "y" : 2.9429874420166016
+        },
+        "rotation" : {
+          "radians" : -3.1024027203970066
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.2301659590513677,
+      "velocity" : 0.9206638362054708,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.792647884092066,
+          "y" : 2.9388182625618144
+        },
+        "rotation" : {
+          "radians" : -3.1018966911012167
+        }
+      },
+      "curvature" : 0.009441605420194805
+    }, {
+      "time" : 0.32549598090329745,
+      "velocity" : 1.3019839236131898,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.686791604118646,
+          "y" : 2.9345442011023373
+        },
+        "rotation" : {
+          "radians" : -3.100425032373633
+        }
+      },
+      "curvature" : 0.01823558838667193
+    }, {
+      "time" : 0.3986288451113676,
+      "velocity" : 1.5945153804454704,
+      "acceleration" : 3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 5.580971612904613,
+          "y" : 2.9300674260512527
+        },
+        "rotation" : {
+          "radians" : -3.098055153290077
+        }
+      },
+      "curvature" : 0.026416758928169817
+    }, {
+      "time" : 0.4602561477805154,
+      "velocity" : 1.8410245911220613,
+      "acceleration" : 3.9999999999999916,
+      "pose" : {
+        "translation" : {
+          "x" : 5.475217593972275,
+          "y" : 2.9252976053729753
+        },
+        "rotation" : {
+          "radians" : -3.094851059223684
+        }
+      },
+      "curvature" : 0.0340260625248394
+    }, {
+      "time" : 0.5145132056228292,
+      "velocity" : 2.0580528224913164,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.369566605466148,
+          "y" : 2.9201516955883897
+        },
+        "rotation" : {
+          "radians" : -3.090872945078929
+        }
+      },
+      "curvature" : 0.04110875647986678
+    }, {
+      "time" : 0.563515762369635,
+      "velocity" : 2.2540630494785394,
+      "acceleration" : 4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 5.264062661760839,
+          "y" : 2.914553730779989
+        },
+        "rotation" : {
+          "radians" : -3.086176913297493
+        }
+      },
+      "curvature" : 0.047712969032265094
+    }, {
+      "time" : 0.6085162323222143,
+      "velocity" : 2.4340649292888568,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.158756315068919,
+          "y" : 2.9084346115970146
+        },
+        "rotation" : {
+          "radians" : -3.080814792156677
+        }
+      },
+      "curvature" : 0.05388859471718488
+    }, {
+      "time" : 0.6503267256223321,
+      "velocity" : 2.601306902489328,
+      "acceleration" : 3.9999999999999956,
+      "pose" : {
+        "translation" : {
+          "x" : 5.053704237048805,
+          "y" : 2.9017318942605925
+        },
+        "rotation" : {
+          "radians" : -3.0748340330236092
+        }
+      },
+      "curvature" : 0.059686480108153514
+    }, {
+      "time" : 0.6895078614844453,
+      "velocity" : 2.758031445937781,
+      "acceleration" : 3.9999999999999956,
+      "pose" : {
+        "translation" : {
+          "x" : 4.948968800412642,
+          "y" : 2.894389579568873
+        },
+        "rotation" : {
+          "radians" : -3.0682776682148387
+        }
+      },
+      "curvature" : 0.06515785747811133
+    }, {
+      "time" : 0.7264647120959763,
+      "velocity" : 2.9058588483839047,
+      "acceleration" : 2.666249486168943,
+      "pose" : {
+        "translation" : {
+          "x" : 4.844617660534176,
+          "y" : 2.8863579019021692
+        },
+        "rotation" : {
+          "radians" : -3.061184313833844
+        }
+      },
+      "curvature" : 0.07035398883055903
+    }, {
+      "time" : 0.7617731677016681,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.556674489449135E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.740723337056639,
+          "y" : 2.8775931182280945
+        },
+        "rotation" : {
+          "radians" : -3.0535882043682183
+        }
+      },
+      "curvature" : 0.07532598833602047
+    }, {
+      "time" : 0.7963729965952918,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.637362795500621,
+          "y" : 2.868057297106702
+        },
+        "rotation" : {
+          "radians" : -3.045519247910405
+        }
+      },
+      "curvature" : 0.08012479693513168
+    }, {
+      "time" : 0.8307945521766903,
+      "velocity" : 3.0,
+      "acceleration" : -8.652000136404901E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.534617028871959,
+          "y" : 2.857718107695623
+        },
+        "rotation" : {
+          "radians" : -3.0370030926303286
+        }
+      },
+      "curvature" : 0.08480128843727726
+    }, {
+      "time" : 0.8650131685157781,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.710165382862835E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.432570639269606,
+          "y" : 2.846548608755203
+        },
+        "rotation" : {
+          "radians" : -3.028061196597019
+        }
+      },
+      "curvature" : 0.08940649170847131
+    }, {
+      "time" : 0.899003277798418,
+      "velocity" : 3.0,
+      "acceleration" : -0.09574616288107782,
+      "pose" : {
+        "translation" : {
+          "x" : 4.331311419493517,
+          "y" : 2.834527037653644
+        },
+        "rotation" : {
+          "radians" : -3.018710894247568
+        }
+      },
+      "curvature" : 0.09399191846371814
+    }, {
+      "time" : 0.9327567117085453,
+      "velocity" : 2.9967682382190453,
+      "acceleration" : -0.16603740413403223,
+      "pose" : {
+        "translation" : {
+          "x" : 4.230929934652522,
+          "y" : 2.821636599372141
+        },
+        "rotation" : {
+          "radians" : -3.0089654537653083
+        }
+      },
+      "curvature" : 0.0986099907832933
+    }, {
+      "time" : 0.9662773051518138,
+      "velocity" : 2.9912025658986927,
+      "acceleration" : -0.17374271650320738,
+      "pose" : {
+        "translation" : {
+          "x" : 4.131519103772213,
+          "y" : 2.807865255510019
+        },
+        "rotation" : {
+          "radians" : -2.9988341203843856
+        }
+      },
+      "curvature" : 0.10331456682545719
+    }, {
+      "time" : 0.9995509183178184,
+      "velocity" : 2.985421517959354,
+      "acceleration" : -0.18358691510613726,
+      "pose" : {
+        "translation" : {
+          "x" : 4.033173781402816,
+          "y" : 2.793205513289873
+        },
+        "rotation" : {
+          "radians" : -2.9883221412125427
+        }
+      },
+      "curvature" : 0.10816156739888892
+    }, {
+      "time" : 1.0325512170508016,
+      "velocity" : 2.979363094917385,
+      "acceleration" : -0.1957881485667769,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9359903392270725,
+          "y" : 2.7776542145627077
+        },
+        "rotation" : {
+          "radians" : -2.977430767583505
+        }
+      },
+      "curvature" : 0.11320971018612586
+    }, {
+      "time" : 1.0652520605900002,
+      "velocity" : 2.9729606573042733,
+      "acceleration" : -0.2106132308438905,
+      "pose" : {
+        "translation" : {
+          "x" : 3.840066247668119,
+          "y" : 2.761212324813073
+        },
+        "rotation" : {
+          "radians" : -2.9661572312387867
+        }
+      },
+      "curvature" : 0.11852136257591563
+    }, {
+      "time" : 1.0976276482842051,
+      "velocity" : 2.966141930179527,
+      "acceleration" : -0.22838513501532773,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7454996574973665,
+          "y" : 2.743884722164204
+        },
+        "rotation" : {
+          "radians" : -2.9544946908190806
+        }
+      },
+      "curvature" : 0.1241635283632123
+    }, {
+      "time" : 1.1296526738305466,
+      "velocity" : 2.9588278903962566,
+      "acceleration" : -0.2494917017630424,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6523889814423782,
+          "y" : 2.7256799863831596
+        },
+        "rotation" : {
+          "radians" : -2.942432145241093
+        }
+      },
+      "curvature" : 0.1302089880914559
+    }, {
+      "time" : 1.1613024877721105,
+      "velocity" : 2.950931524455492,
+      "acceleration" : -0.27439562802342504,
+      "pose" : {
+        "translation" : {
+          "x" : 3.560832475794749,
+          "y" : 2.7066101878859605
+        },
+        "rotation" : {
+          "radians" : -2.9299543105725916
+        }
+      },
+      "curvature" : 0.13673761759648564
+    }, {
+      "time" : 1.1925532689948115,
+      "velocity" : 2.9423564467156664,
+      "acceleration" : -0.30364572614284985,
+      "pose" : {
+        "translation" : {
+          "x" : 3.470927822017984,
+          "y" : 2.686690676742728
+        },
+        "rotation" : {
+          "radians" : -2.9170414570270506
+        }
+      },
+      "curvature" : 0.1438379143723059
+    }, {
+      "time" : 1.2233822059663813,
+      "velocity" : 2.932995371762722,
+      "acceleration" : -0.3378893144869457,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3827717083553797,
+          "y" : 2.6659398716828218
+        },
+        "rotation" : {
+          "radians" : -2.9036692027182647
+        }
+      },
+      "curvature" : 0.15160876664672418
+    }, {
+      "time" : 1.253767688448565,
+      "velocity" : 2.922728441916462,
+      "acceleration" : -0.377885389511768,
+      "pose" : {
+        "translation" : {
+          "x" : 3.296459411437901,
+          "y" : 2.644379049099978
+        },
+        "rotation" : {
+          "radians" : -2.8898082608960753
+        }
+      },
+      "curvature" : 0.1601615053363712
+    }, {
+      "time" : 1.2836895103731156,
+      "velocity" : 2.9114214225836013,
+      "acceleration" : -0.4245178992880957,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2120843778920607,
+          "y" : 2.62203213205745
+        },
+        "rotation" : {
+          "radians" : -2.875424137597093
+        }
+      },
+      "curvature" : 0.1696222839550847
+    }, {
+      "time" : 1.313129084481963,
+      "velocity" : 2.898923796426977,
+      "acceleration" : -0.47880793939403443,
+      "pose" : {
+        "translation" : {
+          "x" : 3.129737805947798,
+          "y" : 2.5989254792931433
+        },
+        "rotation" : {
+          "radians" : -2.860476777086651
+        }
+      },
+      "curvature" : 0.180134835376471
+    }, {
+      "time" : 1.3420696691632106,
+      "velocity" : 2.8850668147108904,
+      "acceleration" : -0.5419229606175602,
+      "pose" : {
+        "translation" : {
+          "x" : 3.049508227046359,
+          "y" : 2.5750876742247573
+        },
+        "rotation" : {
+          "radians" : -2.844920153276735
+        }
+      },
+      "curvature" : 0.19186365592042148
+    }, {
+      "time" : 1.37049660762701,
+      "velocity" : 2.869661604057295,
+      "acceleration" : -0.6151800394568323,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9714810874481756,
+          "y" : 2.550549313954921
+        },
+        "rotation" : {
+          "radians" : -2.828701806670222
+        }
+      },
+      "curvature" : 0.2049976646319762
+    }, {
+      "time" : 1.3983975791037442,
+      "velocity" : 2.852497483323354,
+      "acceleration" : -0.7000388399543165,
+      "pose" : {
+        "translation" : {
+          "x" : 2.895738329840743,
+          "y" : 2.5253427982763332
+        },
+        "rotation" : {
+          "radians" : -2.8117623285706195
+        }
+      },
+      "curvature" : 0.21975437583412769
+    }, {
+      "time" : 1.4257628610363704,
+      "velocity" : 2.8333407231042154,
+      "acceleration" : -0.8320723157548291,
+      "pose" : {
+        "translation" : {
+          "x" : 2.822357974946499,
+          "y" : 2.499502118676901
+        },
+        "rotation" : {
+          "radians" : -2.7940347976772673
+        }
+      },
+      "curvature" : 0.23638460141375914
+    }, {
+      "time" : 1.4525899504690478,
+      "velocity" : 2.8110186446750056,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.751413703130705,
+          "y" : 2.473062647344876
+        },
+        "rotation" : {
+          "radians" : -2.7754441792642495
+        }
+      },
+      "curvature" : 0.25517765880922155
+    }, {
+      "time" : 1.4792695579561794,
+      "velocity" : 2.7043002147264787,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6829744360093244,
+          "y" : 2.4460609261739954
+        },
+        "rotation" : {
+          "radians" : -2.7559067046024177
+        }
+      },
+      "curvature" : 0.27646699088034715
+    }, {
+      "time" : 1.506205081648366,
+      "velocity" : 2.5965581199577317,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.617103918056899,
+          "y" : 2.418534455768619
+        },
+        "rotation" : {
+          "radians" : -2.7353292590414324
+        }
+      },
+      "curvature" : 0.30063598962569904
+    }, {
+      "time" : 1.533414427222619,
+      "velocity" : 2.4877207376607204,
+      "acceleration" : -3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 2.553860298214431,
+          "y" : 2.3905214844488683
+        },
+        "rotation" : {
+          "radians" : -2.713608822413696
+        }
+      },
+      "curvature" : 0.32812363568576425
+    }, {
+      "time" : 1.5609222653091537,
+      "velocity" : 2.3776893853145813,
+      "acceleration" : -3.949364309865745,
+      "pose" : {
+        "translation" : {
+          "x" : 2.493295711497262,
+          "y" : 2.3620607972557637
+        },
+        "rotation" : {
+          "radians" : -2.690632026659688
+        }
+      },
+      "curvature" : 0.35942929054988465
+    }, {
+      "time" : 1.5887534138112722,
+      "velocity" : 2.267774040717741,
+      "acceleration" : -3.7932782770182274,
+      "pose" : {
+        "translation" : {
+          "x" : 2.43545586060295,
+          "y" : 2.333191504956364
+        },
+        "rotation" : {
+          "radians" : -2.6662749246206054
+        }
+      },
+      "curvature" : 0.3951155704482967
+    }, {
+      "time" : 1.6454161870589727,
+      "velocity" : 2.0528363738416293,
+      "acceleration" : -3.524370787870329,
+      "pose" : {
+        "translation" : {
+          "x" : 2.328098505131493,
+          "y" : 2.2743839107679378
+        },
+        "rotation" : {
+          "radians" : -2.612872321869423
+        }
+      },
+      "curvature" : 0.4821864866479269
+    }, {
+      "time" : 1.7034881688217829,
+      "velocity" : 1.8481691777230431,
+      "acceleration" : -3.1886854801463445,
+      "pose" : {
+        "translation" : {
+          "x" : 2.232009308124283,
+          "y" : 2.214410084736084
+        },
+        "rotation" : {
+          "radians" : -2.552217385705653
+        }
+      },
+      "curvature" : 0.5948949158111395
+    }, {
+      "time" : 1.7629801786134969,
+      "velocity" : 1.658467869915481,
+      "acceleration" : -2.793593360246646,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1472796517252846,
+          "y" : 2.153573117658766
+        },
+        "rotation" : {
+          "radians" : -2.4830442955743517
+        }
+      },
+      "curvature" : 0.7387705578516988
+    }, {
+      "time" : 1.823812432610913,
+      "velocity" : 1.4885272890594612,
+      "acceleration" : -2.4614261655202934,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0738578353503314,
+          "y" : 2.092161056443846
+        },
+        "rotation" : {
+          "radians" : -2.404174055212181
+        }
+      },
+      "curvature" : 0.9170860803614409
+    }, {
+      "time" : 1.8547141636761173,
+      "velocity" : 1.4124649596556962,
+      "acceleration" : -2.226093041455955,
+      "pose" : {
+        "translation" : {
+          "x" : 2.041328791717895,
+          "y" : 2.0613236050452572
+        },
+        "rotation" : {
+          "radians" : -2.3608351537787935
+        }
+      },
+      "curvature" : 1.0185171315348982
+    }, {
+      "time" : 1.8858592963049856,
+      "velocity" : 1.3431329966353498,
+      "acceleration" : -1.9794032204108298,
+      "pose" : {
+        "translation" : {
+          "x" : 2.011535687139258,
+          "y" : 2.0304401522735134
+        },
+        "rotation" : {
+          "radians" : -2.314846284252715
+        }
+      },
+      "curvature" : 1.1263819090275555
+    }, {
+      "time" : 1.9171879104709784,
+      "velocity" : 1.2811210368641754,
+      "acceleration" : -1.7210458088826468,
+      "pose" : {
+        "translation" : {
+          "x" : 1.98442585493639,
+          "y" : 1.9995394662212282
+        },
+        "rotation" : {
+          "radians" : -2.26626721249112
+        }
+      },
+      "curvature" : 1.2380648061508586
+    }, {
+      "time" : 1.9486228942778938,
+      "velocity" : 1.22701998973099,
+      "acceleration" : -1.4497718123734797,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9599351754080274,
+          "y" : 1.9686481087687149
+        },
+        "rotation" : {
+          "radians" : -2.2152625603384335
+        }
+      },
+      "curvature" : 1.3496477269860494
+    }, {
+      "time" : 1.9800679118911164,
+      "velocity" : 1.1814318895557523,
+      "acceleration" : -1.1629063029917064,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9379876574375503,
+          "y" : 1.9377902245891243
+        },
+        "rotation" : {
+          "radians" : -2.162125525962242
+        }
+      },
+      "curvature" : 1.4558154532790182
+    }, {
+      "time" : 2.0114057081537027,
+      "velocity" : 1.144988968760121,
+      "acceleration" : -0.8555700128978933,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9184950201008633,
+          "y" : 1.9069873301535836
+        },
+        "rotation" : {
+          "radians" : -2.1072976365024556
+        }
+      },
+      "curvature" : 1.5499621702421391
+    }, {
+      "time" : 2.042496890239119,
+      "velocity" : 1.1183882857022904,
+      "acceleration" : -0.5193738693932407,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9013562742742751,
+          "y" : 1.8762581027363336
+        },
+        "rotation" : {
+          "radians" : -2.051380200524095
+        }
+      },
+      "curvature" : 1.6245702075644999
+    }, {
+      "time" : 2.0731792244008695,
+      "velocity" : 1.1024526830866856,
+      "acceleration" : -0.1401133909627551,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8864573042423771,
+          "y" : 1.8456181694198683
+        },
+        "rotation" : {
+          "radians" : -1.9951332453181105
+        }
+      },
+      "curvature" : 1.6718749322032591
+    }, {
+      "time" : 2.103267289985402,
+      "velocity" : 1.098236942190127,
+      "acceleration" : 0.3065421531438961,
+      "pose" : {
+        "translation" : {
+          "x" : 1.873670449305921,
+          "y" : 1.8150798961000731
+        },
+        "rotation" : {
+          "radians" : -1.939459195747904
+        }
+      },
+      "curvature" : 1.6847350337062899
+    }, {
+      "time" : 2.132552032689131,
+      "velocity" : 1.1072139502727931,
+      "acceleration" : 0.8636627394205474,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8628540853896993,
+          "y" : 1.7846521764913632
+        },
+        "rotation" : {
+          "radians" : -1.885371407123151
+        }
+      },
+      "curvature" : 1.6575269774282888
+    }, {
+      "time" : 2.1607993444095768,
+      "velocity" : 1.1316101008945396,
+      "acceleration" : 1.6117144791019087,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8538522066504246,
+          "y" : 1.7543402211318213
+        },
+        "rotation" : {
+          "radians" : -1.8339512599110672
+        }
+      },
+      "curvature" : 1.5868287956600426
+    }, {
+      "time" : 2.1877462109747206,
+      "velocity" : 1.175040755904009,
+      "acceleration" : 3.6208105431392568,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8464940070846083,
+          "y" : 1.7241453463883367
+        },
+        "rotation" : {
+          "radians" : -1.7863006275868372
+        }
+      },
+      "curvature" : 1.4716950974629326
+    }, {
+      "time" : 2.236042382237465,
+      "velocity" : 1.349912042005413,
+      "acceleration" : 3.9999999999999813,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8359489103056603,
+          "y" : 1.6640913673919613
+        },
+        "rotation" : {
+          "radians" : -1.7065641844274897
+        }
+      },
+      "curvature" : 1.11509729054641
+    }, {
+      "time" : 2.277907500803531,
+      "velocity" : 1.517372516269675,
+      "acceleration" : 3.9833148393409696,
+      "pose" : {
+        "translation" : {
+          "x" : 1.829540444920319,
+          "y" : 1.6044148692087425
+        },
+        "rotation" : {
+          "radians" : -1.654002070934546
+        }
+      },
+      "curvature" : 0.613882276060416
+    }, {
+      "time" : 2.315345763310788,
+      "velocity" : 1.6665009028739752,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 1.825326681137085,
+          "y" : 1.5449646711349487
+        },
+        "rotation" : {
+          "radians" : -1.6352154187320553
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 2.3528636610794607,
+      "velocity" : 1.5164293117992842,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8219009519815899,
+          "y" : 1.485354601351787
+        },
+        "rotation" : {
+          "radians" : -1.6145927519610666
+        }
+      },
+      "curvature" : 0.6613132762682022
+    }, {
+      "time" : 2.3953009609475626,
+      "velocity" : 1.3466801123268766,
+      "acceleration" : -2.447765685928921,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8207798104572248,
+          "y" : 1.4246136307510824
+        },
+        "rotation" : {
+          "radians" : -1.559258462845717
+        }
+      },
+      "curvature" : 1.12045601019587
+    }, {
+      "time" : 2.4441469639679103,
+      "velocity" : 1.2271165422388892,
+      "acceleration" : -0.5720634881939421,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8239080891303416,
+          "y" : 1.361831680281739
+        },
+        "rotation" : {
+          "radians" : -1.480370605655411
+        }
+      },
+      "curvature" : 1.349435348223791
+    }, {
+      "time" : 2.4708676658243456,
+      "velocity" : 1.2118306043279066,
+      "acceleration" : 0.15979441326309798,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8275716032411027,
+          "y" : 1.3294530877098598
+        },
+        "rotation" : {
+          "radians" : -1.43570297822473
+        }
+      },
+      "curvature" : 1.3836934054080776
+    }, {
+      "time" : 2.498501305753145,
+      "velocity" : 1.2162463056066528,
+      "acceleration" : 0.7244111612153562,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8328603306872537,
+          "y" : 1.2963242800469743
+        },
+        "rotation" : {
+          "radians" : -1.389335893805699
+        }
+      },
+      "curvature" : 1.3736643754703268
+    }, {
+      "time" : 2.526759486377394,
+      "velocity" : 1.2367168470464982,
+      "acceleration" : 1.1926306446746133,
+      "pose" : {
+        "translation" : {
+          "x" : 1.839917232537418,
+          "y" : 1.2623921911031442
+        },
+        "rotation" : {
+          "radians" : -1.3424216946735859
+        }
+      },
+      "curvature" : 1.3285660480051897
+    }, {
+      "time" : 2.5553998339251613,
+      "velocity" : 1.270874203206097,
+      "acceleration" : 1.601933018243381,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8488651751439278,
+          "y" : 1.2276157532543266
+        },
+        "rotation" : {
+          "radians" : -1.2959209814340706
+        }
+      },
+      "curvature" : 1.258109886245163
+    }, {
+      "time" : 2.5842198818567117,
+      "velocity" : 1.317041989575004,
+      "acceleration" : 1.9730587889589195,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8598076922431273,
+          "y" : 1.191965371940812
+        },
+        "rotation" : {
+          "radians" : -1.2505887693826183
+        }
+      },
+      "curvature" : 1.171451900081923
+    }, {
+      "time" : 2.613052469764473,
+      "velocity" : 1.3739303805548424,
+      "acceleration" : 2.4837574247785343,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8728297470556754,
+          "y" : 1.1554224001656621
+        },
+        "rotation" : {
+          "radians" : -1.206979322113377
+        }
+      },
+      "curvature" : 1.0764509613846949
+    }, {
+      "time" : 2.670135599245487,
+      "velocity" : 1.5157110272329066,
+      "acceleration" : 3.0990658562394877,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9053640427268448,
+          "y" : 1.0796356820471829
+        },
+        "rotation" : {
+          "radians" : -1.1262654169222006
+        }
+      },
+      "curvature" : 0.8844858371105864
+    }, {
+      "time" : 2.7259954044656856,
+      "velocity" : 1.6888242423270121,
+      "acceleration" : 3.649016814110955,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9468053174205124,
+          "y" : 1.000305405119434
+        },
+        "rotation" : {
+          "radians" : -1.0550981178089267
+        }
+      },
+      "curvature" : 0.7124506625993454
+    }, {
+      "time" : 2.7801731094475235,
+      "velocity" : 1.8865195987556824,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9972424725681748,
+          "y" : 0.9176229045073114
+        },
+        "rotation" : {
+          "radians" : -0.9932815877369497
+        }
+      },
+      "curvature" : 0.5709539223900668
+    }, {
+      "time" : 2.8325182827204705,
+      "velocity" : 2.0959002918474696,
+      "acceleration" : 3.999999999999984,
+      "pose" : {
+        "translation" : {
+          "x" : 2.05654044297944,
+          "y" : 0.8319042281900693
+        },
+        "rotation" : {
+          "radians" : -0.939869543584731
+        }
+      },
+      "curvature" : 0.45987023815540634
+    }, {
+      "time" : 2.8832022831916913,
+      "velocity" : 2.298636293732353,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 2.124364584051719,
+          "y" : 0.7435733209513273
+        },
+        "rotation" : {
+          "radians" : -0.8936544133402136
+        }
+      },
+      "curvature" : 0.37479587351455695
+    }, {
+      "time" : 2.9324371813867773,
+      "velocity" : 2.4955758865126962,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2002050589799182,
+          "y" : 0.6531452083290787
+        },
+        "rotation" : {
+          "radians" : -0.853427174289811
+        }
+      },
+      "curvature" : 0.3104667355759126
+    }, {
+      "time" : 2.980286705128464,
+      "velocity" : 2.686973981479444,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2834012259661307,
+          "y" : 0.5612091805656974
+        },
+        "rotation" : {
+          "radians" : -0.8180911393767909
+        }
+      },
+      "curvature" : 0.26216926405981567
+    }, {
+      "time" : 3.0036886028665157,
+      "velocity" : 2.7805815724316503,
+      "acceleration" : 1.7834018615548795,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3275160092188085,
+          "y" : 0.514876020137125
+        },
+        "rotation" : {
+          "radians" : -0.8019547141012324
+        }
+      },
+      "curvature" : 0.24281777874977586
+    }, {
+      "time" : 3.026940898356585,
+      "velocity" : 2.822049759494064,
+      "acceleration" : 0.6908039852530397,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3731660254293274,
+          "y" : 0.4684119765579453
+        },
+        "rotation" : {
+          "radians" : -0.7866964853329266
+        }
+      },
+      "curvature" : 0.22613732914460744
+    }, {
+      "time" : 3.050322026787958,
+      "velocity" : 2.8382015361941697,
+      "acceleration" : 0.5944336707391082,
+      "pose" : {
+        "translation" : {
+          "x" : 2.420237140398036,
+          "y" : 0.4219041096592999
+        },
+        "rotation" : {
+          "radians" : -0.772220104481907
+        }
+      },
+      "curvature" : 0.2118009615897061
+    }, {
+      "time" : 3.0738960629383696,
+      "velocity" : 2.8522147370371953,
+      "acceleration" : 0.5085625308709665,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4686103672150495,
+          "y" : 0.37544096780698055
+        },
+        "rotation" : {
+          "radians" : -0.7584370845254595
+        }
+      },
+      "curvature" : 0.19952992215767287
+    }, {
+      "time" : 3.097629651885521,
+      "velocity" : 2.86428475109881,
+      "acceleration" : 0.4316967127192361,
+      "pose" : {
+        "translation" : {
+          "x" : 2.518162628360552,
+          "y" : 0.3291120623998669
+        },
+        "rotation" : {
+          "radians" : -0.7452660870351325
+        }
+      },
+      "curvature" : 0.18908811609138015
+    }, {
+      "time" : 3.1214873211538543,
+      "velocity" : 2.874584028495092,
+      "acceleration" : 0.3624475659523509,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5687675178050995,
+          "y" : 0.28300734236836433
+        },
+        "rotation" : {
+          "radians" : -0.7326322058073411
+        }
+      },
+      "curvature" : 0.1802768862425938
+    }, {
+      "time" : 3.145431891692516,
+      "velocity" : 2.8832626798046044,
+      "acceleration" : 0.2995436811273533,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6202960631099237,
+          "y" : 0.23721666867284164
+        },
+        "rotation" : {
+          "radians" : -0.7204662745977262
+        }
+      },
+      "curvature" : 0.17293027048351933
+    }, {
+      "time" : 3.1694248840625208,
+      "velocity" : 2.890449629060376,
+      "acceleration" : 0.24183189107987368,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6726174875272335,
+          "y" : 0.19182928880206873
+        },
+        "rotation" : {
+          "radians" : -0.708704216367118
+        }
+      },
+      "curvature" : 0.16691079279926538
+    }, {
+      "time" : 3.193426917481795,
+      "velocity" : 2.8962540861919215,
+      "acceleration" : 0.18827218906255389,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7255999721005186,
+          "y" : 0.14693331127165443
+        },
+        "rotation" : {
+          "radians" : -0.6972864449360306
+        }
+      },
+      "curvature" : 0.16210577857562763
+    }, {
+      "time" : 3.217398100152016,
+      "velocity" : 2.9007671932276624,
+      "acceleration" : 0.13793017248481926,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7791114177648524,
+          "y" : 0.10261518012248416
+        },
+        "rotation" : {
+          "radians" : -0.6861573260641398
+        }
+      },
+      "curvature" : 0.15842414582671857
+    }, {
+      "time" : 3.241298409734759,
+      "velocity" : 2.9040637670508507,
+      "acceleration" : 0.08996972588049867,
+      "pose" : {
+        "translation" : {
+          "x" : 2.8330202074471944,
+          "y" : 0.0589591494191577
+        },
+        "rotation" : {
+          "radians" : -0.6752647030503031
+        }
+      },
+      "curvature" : 0.1557936000148212
+    }, {
+      "time" : 3.2650880630305976,
+      "velocity" : 2.9062041156366694,
+      "acceleration" : 0.04364814484804298,
+      "pose" : {
+        "translation" : {
+          "x" : 2.8871959681666937,
+          "y" : 0.0160467577484269
+        },
+        "rotation" : {
+          "radians" : -0.6645594915329975
+        }
+      },
+      "curvature" : 0.15415814328341032
+    }, {
+      "time" : 3.288727873894095,
+      "velocity" : 2.9072359495254196,
+      "acceleration" : -0.0016842852381663946,
+      "pose" : {
+        "translation" : {
+          "x" : 2.941510333134991,
+          "y" : -0.02604369728236655
+        },
+        "rotation" : {
+          "radians" : -0.6539953489714027
+        }
+      },
+      "curvature" : 0.15347579410401332
+    }, {
+      "time" : 3.3121795982222406,
+      "velocity" : 2.907196450132324,
+      "acceleration" : -0.046578151707487234,
+      "pose" : {
+        "translation" : {
+          "x" : 2.995837703856523,
+          "y" : -0.06723768454685342
+        },
+        "rotation" : {
+          "radians" : -0.6435284261390796
+        }
+      },
+      "curvature" : 0.15371639662570263
+    }, {
+      "time" : 3.335406264503977,
+      "velocity" : 2.9061145949465943,
+      "acceleration" : -0.09146977902103462,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0500560122288234,
+          "y" : -0.10746496490119917
+        },
+        "rotation" : {
+          "radians" : -0.6331172107927046
+        }
+      },
+      "curvature" : 0.15485937730919708
+    }, {
+      "time" : 3.358372487917268,
+      "velocity" : 2.904013879566033,
+      "acceleration" : -0.13665410312300072,
+      "pose" : {
+        "translation" : {
+          "x" : 3.1040474826428266,
+          "y" : -0.14666011668566625
+        },
+        "rotation" : {
+          "radians" : -0.6227224774786422
+        }
+      },
+      "curvature" : 0.15689127710744225
+    }, {
+      "time" : 3.381044765324835,
+      "velocity" : 2.900915619831146,
+      "acceleration" : -2.1491915946775815,
+      "pose" : {
+        "translation" : {
+          "x" : 3.157699394083171,
+          "y" : -0.18476306122617636
+        },
+        "rotation" : {
+          "radians" : -0.6123073622363067
+        }
+      },
+      "curvature" : 0.15980284830484542
+    }, {
+      "time" : 3.403563912604914,
+      "velocity" : 2.8525176577774944,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2109048422285014,
+          "y" : -0.22171958833587269
+        },
+        "rotation" : {
+          "radians" : -0.6018375867750714
+        }
+      },
+      "curvature" : 0.16358545453108617
+    }, {
+      "time" : 3.4492280218552347,
+      "velocity" : 2.6698612207762125,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 3.315582387420548,
+          "y" : -0.2920090449608779
+        },
+        "rotation" : {
+          "radians" : -0.5806125205758887
+        }
+      },
+      "curvature" : 0.1737031415493062
+    }, {
+      "time" : 3.4961580129617453,
+      "velocity" : 2.48214125635017,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4173701773397624,
+          "y" : -0.35723214386962354
+        },
+        "rotation" : {
+          "radians" : -0.558846041472928
+        }
+      },
+      "curvature" : 0.18697261071178092
+    }, {
+      "time" : 3.544441517737483,
+      "velocity" : 2.2890072372472194,
+      "acceleration" : -4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5157001126304266,
+          "y" : -0.4172200702665805
+        },
+        "rotation" : {
+          "radians" : -0.5364315370742367
+        }
+      },
+      "curvature" : 0.20264494363905447
+    }, {
+      "time" : 3.594311089348581,
+      "velocity" : 2.0895289508028285,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.610170322670001,
+          "y" : -0.471948353301741
+        },
+        "rotation" : {
+          "radians" : -0.5134162602944821
+        }
+      },
+      "curvature" : 0.2190045005586516
+    }, {
+      "time" : 3.6462406863199543,
+      "velocity" : 1.8818105629173347,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7005695527788163,
+          "y" : -0.5215536821206115
+        },
+        "rotation" : {
+          "radians" : -0.4900919671398013
+        }
+      },
+      "curvature" : 0.23265913487493842
+    }, {
+      "time" : 3.701128109083174,
+      "velocity" : 1.6622608718644565,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7869015514297644,
+          "y" : -0.5663507219142048
+        },
+        "rotation" : {
+          "radians" : -0.4671173179465116
+        }
+      },
+      "curvature" : 0.23773186828932136
+    }, {
+      "time" : 3.7606892883012333,
+      "velocity" : 1.4240161549922183,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8694094574579907,
+          "y" : -0.6068489299690327
+        },
+        "rotation" : {
+          "radians" : -0.4456529747120997
+        }
+      },
+      "curvature" : 0.2254664901452733
+    }, {
+      "time" : 3.8285064753472717,
+      "velocity" : 1.1527474068080645,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9486001872705856,
+          "y" : -0.6437693717170987
+        },
+        "rotation" : {
+          "radians" : -0.42745091338906643
+        }
+      },
+      "curvature" : 0.1853952813106193
+    }, {
+      "time" : 3.9140667774226117,
+      "velocity" : 0.8105061985067056,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.025268822056276,
+          "y" : -0.67806153678589
+        },
+        "rotation" : {
+          "radians" : -0.414785534453574
+        }
+      },
+      "curvature" : 0.10957339292803865
+    }, {
+      "time" : 4.116693327049288,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.100522994995117,
+          "y" : -0.7109201550483704
+        },
+        "rotation" : {
+          "radians" : -0.41008727651812055
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.8985185623168945, -6.7758471965789795, 0.0 ],
+      "y" : [ 2.9429874420166016, -0.26568102836608887, 0.0 ]
+    }, {
+      "x" : [ 1.825326681137085, -0.12263345718383789, 0.0 ],
+      "y" : [ 1.5449646711349487, -1.9010478258132935, 0.0 ]
+    }, {
+      "x" : [ 4.100522994995117, 2.3999733924865723, 0.0 ],
+      "y" : [ -0.7109201550483704, -1.0433523654937744, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : -3.1364759703203093
+      }
+    }, {
+      "time" : 2.3099999483674765,
+      "rotation" : {
+        "radians" : 0.022477488801708422
+      }
+    }, {
+      "time" : 4.029999909922481,
+      "rotation" : {
+        "radians" : -0.44615049906220317
+      }
+    } ],
+    "reversed" : false,
+    "color" : 52.47058,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@t 2\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT",
+    "closed" : true,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 2.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "REVERSE" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "EJECT" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "@t 3\nIntake.setWantedIntakeState OFF",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 3.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "OFF" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 4.100522994995117,
+          "y" : -0.7109201550483704
+        },
+        "rotation" : {
+          "radians" : 2.5857927494599444
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.17905554113279049,
+      "velocity" : 0.716222164531162,
+      "acceleration" : 3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 4.046051351866051,
+          "y" : -0.6770905534088945
+        },
+        "rotation" : {
+          "radians" : 2.5859290584771175
+        }
+      },
+      "curvature" : 0.004100780417876861
+    }, {
+      "time" : 0.2532222147861238,
+      "velocity" : 1.0128888591444951,
+      "acceleration" : 3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9915714570599867,
+          "y" : -0.6432753174175616
+        },
+        "rotation" : {
+          "radians" : 2.5862997183209484
+        }
+      },
+      "curvature" : 0.007316179708346309
+    }, {
+      "time" : 0.3101283388167845,
+      "velocity" : 1.2405133552671377,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9370801190687295,
+          "y" : -0.6094880375241134
+        },
+        "rotation" : {
+          "radians" : 2.5868491896599624
+        }
+      },
+      "curvature" : 0.009686266454747228
+    }, {
+      "time" : 0.35809346841854367,
+      "velocity" : 1.4323738736741745,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8825794348939553,
+          "y" : -0.5757414756226353
+        },
+        "rotation" : {
+          "radians" : 2.5875244773924204
+        }
+      },
+      "curvature" : 0.011252124346097734
+    }, {
+      "time" : 0.4003365631482469,
+      "velocity" : 1.6013462525929871,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.828076535205502,
+          "y" : -0.5420475838784169
+        },
+        "rotation" : {
+          "radians" : 2.588275148867047
+        }
+      },
+      "curvature" : 0.012054310762550804
+    }, {
+      "time" : 0.43850530180719144,
+      "velocity" : 1.7540212072287653,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7735833294996617,
+          "y" : -0.5084175235548116
+        },
+        "rotation" : {
+          "radians" : 2.5890532903104244
+        }
+      },
+      "curvature" : 0.0121315702156112
+    }, {
+      "time" : 0.4735755889182925,
+      "velocity" : 1.8943023556731697,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 3.719116251257474,
+          "y" : -0.4748616838400972
+        },
+        "rotation" : {
+          "radians" : 2.589813411890699
+        }
+      },
+      "curvature" : 0.01151977337607476
+    }, {
+      "time" : 0.5061806573270884,
+      "velocity" : 2.024722629308353,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6646960031030176,
+          "y" : -0.4413897006743355
+        },
+        "rotation" : {
+          "radians" : 2.590512310722669
+        }
+      },
+      "curvature" : 0.010251050076436605
+    }, {
+      "time" : 0.5367579667986577,
+      "velocity" : 2.147031867194631,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 3.610347301961702,
+          "y" : -0.4080104755762326
+        },
+        "rotation" : {
+          "radians" : 2.591108899964241
+        }
+      },
+      "curvature" : 0.008353084081511222
+    }, {
+      "time" : 0.5656238557211262,
+      "velocity" : 2.2624954228845047,
+      "acceleration" : 3.9999999999999893,
+      "pose" : {
+        "translation" : {
+          "x" : 3.55609862421856,
+          "y" : -0.37473219446999906
+        },
+        "rotation" : {
+          "radians" : 2.5915640110153895
+        }
+      },
+      "curvature" : 0.0058485380718137905
+    }, {
+      "time" : 0.6191140364610012,
+      "velocity" : 2.476456145844004,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4480325127148035,
+          "y" : -0.308507742918664
+        },
+        "rotation" : {
+          "radians" : 2.5919013866336735
+        }
+      },
+      "curvature" : -9.17525897781337E-4
+    }, {
+      "time" : 0.6679788967836321,
+      "velocity" : 2.671915587134528,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.340790984879593,
+          "y" : -0.24276823694478367
+        },
+        "rotation" : {
+          "radians" : 2.591240769502497
+        }
+      },
+      "curvature" : -0.009982710275132317
+    }, {
+      "time" : 0.7130614555741249,
+      "velocity" : 2.8522458222964993,
+      "acceleration" : 3.5172385867566596,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2347111984854564,
+          "y" : -0.17755532287992537
+        },
+        "rotation" : {
+          "radians" : 2.5893138286617203
+        }
+      },
+      "curvature" : -0.02138799605984592
+    }, {
+      "time" : 0.7550700296469314,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 7.342027890777618E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.130165997854956,
+          "y" : -0.11290100492229271
+        },
+        "rotation" : {
+          "radians" : 2.5858591271774563
+        }
+      },
+      "curvature" : -0.035290971114166826
+    }, {
+      "time" : 0.7953939670709853,
+      "velocity" : 3.0,
+      "acceleration" : -7.485000429419822E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0275557589260416,
+          "y" : -0.04882824759624782
+        },
+        "rotation" : {
+          "radians" : 2.5806134504141105
+        }
+      },
+      "curvature" : -0.05197909891742402
+    }, {
+      "time" : 0.8349476686405554,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : -0.5537636550923151,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9273002343174035,
+          "y" : 0.014648421788166388
+        },
+        "rotation" : {
+          "radians" : 2.573303027054985
+        }
+      },
+      "curvature" : -0.07188677132820555
+    }, {
+      "time" : 0.87374969323478,
+      "velocity" : 2.97851284903572,
+      "acceleration" : -0.9070822146479386,
+      "pose" : {
+        "translation" : {
+          "x" : 2.829830398393824,
+          "y" : 0.07752231067570392
+        },
+        "rotation" : {
+          "radians" : 2.563634738472427
+        }
+      },
+      "curvature" : -0.095617170645365
+    }, {
+      "time" : 0.9118975599042641,
+      "velocity" : 2.94390959765307,
+      "acceleration" : -2.262555771483939,
+      "pose" : {
+        "translation" : {
+          "x" : 2.73558029233153,
+          "y" : 0.13979395880639345
+        },
+        "rotation" : {
+          "radians" : 2.5512874700147776
+        }
+      },
+      "curvature" : -0.12396891770742956
+    }, {
+      "time" : 0.9496761220339536,
+      "velocity" : 2.8584334938681764,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6449788691835465,
+          "y" : 0.20147053575601603
+        },
+        "rotation" : {
+          "radians" : 2.5359039299223154
+        }
+      },
+      "curvature" : -0.15796560005178947
+    }, {
+      "time" : 0.9877492069346551,
+      "velocity" : 2.7061411542653704,
+      "acceleration" : -3.9999999999999956,
+      "pose" : {
+        "translation" : {
+          "x" : 2.558441838945047,
+          "y" : 0.26256523847658286
+        },
+        "rotation" : {
+          "radians" : 2.517083600091246
+        }
+      },
+      "curvature" : -0.19888275461263835
+    }, {
+      "time" : 1.0265482053771444,
+      "velocity" : 2.550945160495413,
+      "acceleration" : -4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4763635136187077,
+          "y" : 0.323096688836813
+        },
+        "rotation" : {
+          "radians" : 2.494378068434297
+        }
+      },
+      "curvature" : -0.2482604413289035
+    }, {
+      "time" : 1.0661195848558005,
+      "velocity" : 2.3926596425807887,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3991086522800593,
+          "y" : 0.383088331162611
+        },
+        "rotation" : {
+          "radians" : 2.4672909353168477
+        }
+      },
+      "curvature" : -0.3078783541033658
+    }, {
+      "time" : 1.1065518464830886,
+      "velocity" : 2.230930596071637,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3270043061428396,
+          "y" : 0.44256782977754483
+        },
+        "rotation" : {
+          "radians" : 2.4352859073052295
+        }
+      },
+      "curvature" : -0.37965241689117946
+    }, {
+      "time" : 1.1479982964991189,
+      "velocity" : 2.065144796007515,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2603316636243456,
+          "y" : 0.5015664665433235
+        },
+        "rotation" : {
+          "radians" : 2.397808651299899
+        }
+      },
+      "curvature" : -0.46538626343634765
+    }, {
+      "time" : 1.1907134105418258,
+      "velocity" : 1.8942843398366878,
+      "acceleration" : -3.8004076554878155,
+      "pose" : {
+        "translation" : {
+          "x" : 2.199317895410786,
+          "y" : 0.5601185384002747
+        },
+        "rotation" : {
+          "radians" : 2.35433030289942
+        }
+      },
+      "curvature" : -0.5662827934211753
+    }, {
+      "time" : 1.2350003317172529,
+      "velocity" : 1.725975985563609,
+      "acceleration" : -3.096610871868397,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1441279995226346,
+          "y" : 0.6182607549078227
+        },
+        "rotation" : {
+          "radians" : 2.304422426440884
+        }
+      },
+      "curvature" : -0.6821096692467903
+    }, {
+      "time" : 1.2808802623624265,
+      "velocity" : 1.5839036935271966,
+      "acceleration" : -2.455893226230777,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0948566463799807,
+          "y" : 0.6760316357849661
+        },
+        "rotation" : {
+          "radians" : 2.24787278990788
+        }
+      },
+      "curvature" : -0.8099648281546794
+    }, {
+      "time" : 1.3280319668435383,
+      "velocity" : 1.4681041418867986,
+      "acceleration" : -1.8499614404769908,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0515200238678837,
+          "y" : 0.7334709084507551
+        },
+        "rotation" : {
+          "radians" : 2.184845091649205
+        }
+      },
+      "curvature" : -0.9427791639774807
+    }, {
+      "time" : 1.376032015282584,
+      "velocity" : 1.3793059031335362,
+      "acceleration" : -1.248435182006515,
+      "pose" : {
+        "translation" : {
+          "x" : 2.014047682401724,
+          "y" : 0.7906189055647701
+        },
+        "rotation" : {
+          "radians" : 2.1160696333836517
+        }
+      },
+      "curvature" : -1.0680768788914654
+    }, {
+      "time" : 1.424334569704738,
+      "velocity" : 1.3190032948121349,
+      "acceleration" : -0.6091506293120736,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9822743799925568,
+          "y" : 0.8475159625675985
+        },
+        "rotation" : {
+          "radians" : 2.0430250430635404
+        }
+      },
+      "curvature" : -1.1679706858124066
+    }, {
+      "time" : 1.4722549198410007,
+      "velocity" : 1.2898125833697756,
+      "acceleration" : 0.14219135021541737,
+      "pose" : {
+        "translation" : {
+          "x" : 1.955931927312463,
+          "y" : 0.9042018152213132
+        },
+        "rotation" : {
+          "radians" : 1.9680433024899477
+        }
+      },
+      "curvature" : -1.221435348671096
+    }, {
+      "time" : 1.5189560337976418,
+      "velocity" : 1.2964530778198344,
+      "acceleration" : 1.1656913546554033,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9346410327599024,
+          "y" : 0.9607149971499496
+        },
+        "rotation" : {
+          "radians" : 1.894265907226921
+        }
+      },
+      "curvature" : -1.2089548933496592
+    }, {
+      "time" : 1.563428663287173,
+      "velocity" : 1.348294437534574,
+      "acceleration" : 2.8841346574602866,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9179031475250667,
+          "y" : 1.017092237379984
+        },
+        "rotation" : {
+          "radians" : 1.8254232181396424
+        }
+      },
+      "curvature" : -1.1177745520566962
+    }, {
+      "time" : 1.604436283065203,
+      "velocity" : 1.4665659349563442,
+      "acceleration" : 3.999999999999992,
+      "pose" : {
+        "translation" : {
+          "x" : 1.905092310655231,
+          "y" : 1.073367857880811
+        },
+        "rotation" : {
+          "radians" : 1.7654947807008987
+        }
+      },
+      "curvature" : -0.9447578680023896
+    }, {
+      "time" : 1.6414523727107633,
+      "velocity" : 1.6146302935385846,
+      "acceleration" : 3.7005826612074797,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8954469941201069,
+          "y" : 1.1295731711052213
+        },
+        "rotation" : {
+          "radians" : 1.7183797533566942
+        }
+      },
+      "curvature" : -0.6949978934363836
+    }, {
+      "time" : 1.706643337288398,
+      "velocity" : 1.855874846721971,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8818800449371338,
+          "y" : 1.2418794631958008
+        },
+        "rotation" : {
+          "radians" : 1.676778825261928
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 1.7723136975830924,
+      "velocity" : 1.5931934055431933,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8715964211490927,
+          "y" : 1.3546623770804445
+        },
+        "rotation" : {
+          "radians" : 1.632957005658238
+        }
+      },
+      "curvature" : -0.7254972162927384
+    }, {
+      "time" : 1.810034487394077,
+      "velocity" : 1.4423102462992545,
+      "acceleration" : -2.710726662081756,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8693755361295565,
+          "y" : 1.4118700819531647
+        },
+        "rotation" : {
+          "radians" : 1.5838413049635738
+        }
+      },
+      "curvature" : -0.9768015183979825
+    }, {
+      "time" : 1.8519046258057839,
+      "velocity" : 1.3288117457615873,
+      "acceleration" : -1.1894816130443073,
+      "pose" : {
+        "translation" : {
+          "x" : 1.870371506531228,
+          "y" : 1.4698751626601734
+        },
+        "rotation" : {
+          "radians" : 1.521732134311164
+        }
+      },
+      "curvature" : -1.1507918750528034
+    }, {
+      "time" : 1.8972897575518486,
+      "velocity" : 1.2748269660440499,
+      "acceleration" : -0.3477506556011637,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8753421949347988,
+          "y" : 1.528748941692104
+        },
+        "rotation" : {
+          "radians" : 1.4504225267826032
+        }
+      },
+      "curvature" : -1.2503201227521346
+    }, {
+      "time" : 1.9450499018565859,
+      "velocity" : 1.2582183445504713,
+      "acceleration" : 0.2016282832037308,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8849010019640673,
+          "y" : 1.5884782075607973
+        },
+        "rotation" : {
+          "radians" : 1.3734581625763413
+        }
+      },
+      "curvature" : -1.28354671036822
+    }, {
+      "time" : 1.9943208439648752,
+      "velocity" : 1.268152760019596,
+      "acceleration" : 0.5969274636685241,
+      "pose" : {
+        "translation" : {
+          "x" : 1.89952602502051,
+          "y" : 1.6489738267635623
+        },
+        "rotation" : {
+          "radians" : 1.293941372408038
+        }
+      },
+      "curvature" : -1.2635154627438518
+    }, {
+      "time" : 2.0444402181362453,
+      "velocity" : 1.2980703909243658,
+      "acceleration" : 0.8935195397370149,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9195692170178518,
+          "y" : 1.710079355747439
+        },
+        "rotation" : {
+          "radians" : 1.2143690380279577
+        }
+      },
+      "curvature" : -1.2059442083489496
+    }, {
+      "time" : 2.094911061001386,
+      "velocity" : 1.3431670752113656,
+      "acceleration" : 1.1141266498975333,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9452655451166372,
+          "y" : 1.7715796528734593
+        },
+        "rotation" : {
+          "radians" : 1.1365566560794031
+        }
+      },
+      "curvature" : -1.1263247530634717
+    }, {
+      "time" : 2.1453768398619966,
+      "velocity" : 1.3993923443478076,
+      "acceleration" : 1.2676595640303998,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9767421494588007,
+          "y" : 1.8332094903809093
+        },
+        "rotation" : {
+          "radians" : 1.061661442713639
+        }
+      },
+      "curvature" : -1.0376352482876874
+    }, {
+      "time" : 2.1955990497229334,
+      "velocity" : 1.4630570090047659,
+      "acceleration" : 1.3576641534621845,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0140275019022376,
+          "y" : 1.8946621663515906
+        },
+        "rotation" : {
+          "radians" : 0.9902801108323195
+        }
+      },
+      "curvature" : -0.949295026502291
+    }, {
+      "time" : 2.2454352355231433,
+      "velocity" : 1.5307178120109917,
+      "acceleration" : 1.3867285924070238,
+      "pose" : {
+        "translation" : {
+          "x" : 2.057060564755375,
+          "y" : 1.9555981166740821
+        },
+        "rotation" : {
+          "radians" : 0.9225815953395504
+        }
+      },
+      "curvature" : -0.8672282806254888
+    }, {
+      "time" : 2.2948177631789672,
+      "velocity" : 1.5991979750766532,
+      "acceleration" : 1.3589820685583802,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1056999495117417,
+          "y" : 2.015653527008002
+        },
+        "rotation" : {
+          "radians" : 0.858440269383883
+        }
+      },
+      "curvature" : -0.7945463578337496
+    }, {
+      "time" : 2.3437341392878004,
+      "velocity" : 1.6656744530674152,
+      "acceleration" : 1.2814168669463941,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1597330755845405,
+          "y" : 2.0744489447482692
+        },
+        "rotation" : {
+          "radians" : 0.7975494729913294
+        }
+      },
+      "curvature" : -0.7323917673147309
+    }, {
+      "time" : 2.3922093962778366,
+      "velocity" : 1.7277914650040085,
+      "acceleration" : 1.1642994999905252,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2188853290412176,
+          "y" : 2.131597890989365
+        },
+        "rotation" : {
+          "radians" : 0.7395085520599514
+        }
+      },
+      "curvature" : -0.6806769668594456
+    }, {
+      "time" : 2.4402906479447277,
+      "velocity" : 1.7837724422786883,
+      "acceleration" : 1.0208386073494335,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2828292213380337,
+          "y" : 2.1867154724895954
+        },
+        "rotation" : {
+          "radians" : 0.6838850476440103
+        }
+      },
+      "curvature" : -0.6386233642001887
+    }, {
+      "time" : 2.4880336196518704,
+      "velocity" : 1.8325103110269312,
+      "acceleration" : 0.8663296664660994,
+      "pose" : {
+        "translation" : {
+          "x" : 2.351193548054635,
+          "y" : 2.2394269936353517
+        },
+        "rotation" : {
+          "radians" : 0.6302573876108508
+        }
+      },
+      "curvature" : -0.6051051506550083
+    }, {
+      "time" : 2.5354908013854165,
+      "velocity" : 1.873623875449575,
+      "acceleration" : 0.7170825323129972,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4235725476286234,
+          "y" : 2.2893765684053733
+        },
+        "rotation" : {
+          "radians" : 0.5782441181597022
+        }
+      },
+      "curvature" : -0.5788404596526515
+    }, {
+      "time" : 2.582700817287668,
+      "velocity" : 1.9074773532032985,
+      "acceleration" : 0.5894907207875506,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4995350600901283,
+          "y" : 2.336235732335009
+        },
+        "rotation" : {
+          "radians" : 0.5275249535906203
+        }
+      },
+      "curvature" : -0.5584765250518172
+    }, {
+      "time" : 2.629678599279987,
+      "velocity" : 1.935170319770951,
+      "acceleration" : 0.49960482001977663,
+      "pose" : {
+        "translation" : {
+          "x" : 2.578633685796376,
+          "y" : 2.3797120544804784
+        },
+        "rotation" : {
+          "radians" : 0.47785770284077494
+        }
+      },
+      "curvature" : -0.5426069030826571
+    }, {
+      "time" : 2.6764059539315443,
+      "velocity" : 1.9585155313816427,
+      "acceleration" : 0.46356554662244476,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6604139441662618,
+          "y" : 2.4195577493831344
+        },
+        "rotation" : {
+          "radians" : 0.42909391062732893
+        }
+      },
+      "curvature" : -0.5297484122773238
+    }, {
+      "time" : 2.7228220905005966,
+      "velocity" : 1.9800324531023774,
+      "acceleration" : 0.4993320852380286,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7444234324149193,
+          "y" : 2.455578289033724
+        },
+        "rotation" : {
+          "radians" : 0.3811949720767674
+        }
+      },
+      "curvature" : -0.5182974673057555
+    }, {
+      "time" : 2.7688136027162047,
+      "velocity" : 2.0029974908002472,
+      "acceleration" : 0.6304938957131165,
+      "pose" : {
+        "translation" : {
+          "x" : 2.8302209842882924,
+          "y" : 2.487641014836651
+        },
+        "rotation" : {
+          "radians" : 0.33424953905468247
+        }
+      },
+      "curvature" : -0.5064806911118291
+    }, {
+      "time" : 2.8142032161774035,
+      "velocity" : 2.031615365016311,
+      "acceleration" : 0.8939968521835171,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9173858287977055,
+          "y" : 2.5156837495742366
+        },
+        "rotation" : {
+          "radians" : 0.28849219299419626
+        }
+      },
+      "curvature" : -0.4923123451071928
+    }, {
+      "time" : 2.85873622335247,
+      "velocity" : 2.0714277332490862,
+      "acceleration" : 1.3565333753864743,
+      "pose" : {
+        "translation" : {
+          "x" : 3.005526748954434,
+          "y" : 2.539723409370982
+        },
+        "rotation" : {
+          "radians" : 0.24432259326770525
+        }
+      },
+      "curvature" : -0.4735699432966268
+    }, {
+      "time" : 2.902062698889524,
+      "velocity" : 2.1302015433529657,
+      "acceleration" : 2.153155459162207,
+      "pose" : {
+        "translation" : {
+          "x" : 3.094291240504276,
+          "y" : 2.5598646156578297
+        },
+        "rotation" : {
+          "radians" : 0.20232364570316902
+        }
+      },
+      "curvature" : -0.4477981691549623
+    }, {
+      "time" : 2.9437117576976832,
+      "velocity" : 2.2198784416947213,
+      "acceleration" : 3.592461951491249,
+      "pose" : {
+        "translation" : {
+          "x" : 3.183374670662122,
+          "y" : 2.5763083071364257
+        },
+        "rotation" : {
+          "radians" : 0.1632767221779342
+        }
+      },
+      "curvature" : -0.4123493520484391
+    }, {
+      "time" : 2.9830497106114113,
+      "velocity" : 2.3611985407868445,
+      "acceleration" : 4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2725294368465256,
+          "y" : 2.5893603517433803
+        },
+        "rotation" : {
+          "radians" : 0.12817160720952014
+        }
+      },
+      "curvature" : -0.36446740220852936
+    }, {
+      "time" : 3.0198548135911913,
+      "velocity" : 2.508418952705964,
+      "acceleration" : 4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3615741254142755,
+          "y" : 2.599440158614531
+        },
+        "rotation" : {
+          "radians" : 0.0982084966435118
+        }
+      },
+      "curvature" : -0.30142499142103213
+    }, {
+      "time" : 3.0544441058745386,
+      "velocity" : 2.646776121839353,
+      "acceleration" : 3.99999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 3.450402670394965,
+          "y" : 2.6070892900492026
+        },
+        "rotation" : {
+          "radians" : 0.07478856387691135
+        }
+      },
+      "curvature" : -0.22073732623662828
+    }, {
+      "time" : 3.087179502133278,
+      "velocity" : 2.7777177068743097,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.538993512225563,
+          "y" : 2.6129800734744713
+        },
+        "rotation" : {
+          "radians" : 0.05948748242671234
+        }
+      },
+      "curvature" : -0.1205067561761745
+    }, {
+      "time" : 3.118362856162708,
+      "velocity" : 2.9024511229920305,
+      "acceleration" : 3.2578770345335637,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6274187564849854,
+          "y" : 2.617924213409424
+        },
+        "rotation" : {
+          "radians" : 0.0540017456962338
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 3.148305323595932,
+      "velocity" : 3.0,
+      "acceleration" : -1.0150458753684738E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 3.715657867815885,
+          "y" : 2.6226762181125665
+        },
+        "rotation" : {
+          "radians" : 0.05341425500982433
+        }
+      },
+      "curvature" : -0.012753175869738934
+    }, {
+      "time" : 3.1774724263267227,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.0337019039886706E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 3.803037680920397,
+          "y" : 2.6272856863245124
+        },
+        "rotation" : {
+          "radians" : 0.05184775131197311
+        }
+      },
+      "curvature" : -0.022669788781850372
+    }, {
+      "time" : 3.2061131274893104,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.888849129236597,
+          "y" : 2.631644952964443
+        },
+        "rotation" : {
+          "radians" : 0.04955597015846323
+        }
+      },
+      "curvature" : -0.03035674043346665
+    }, {
+      "time" : 3.2340415748283977,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9725371670283494,
+          "y" : 2.6356817669948214
+        },
+        "rotation" : {
+          "radians" : 0.04676376501044413
+        }
+      },
+      "curvature" : -0.03596921557348524
+    }, {
+      "time" : 3.261120207908762,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.053689914296321,
+          "y" : 2.639356401176549
+        },
+        "rotation" : {
+          "radians" : 0.043689807233283716
+        }
+      },
+      "curvature" : -0.03932125691085411
+    }, {
+      "time" : 3.2872560287661248,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.1320278016889915,
+          "y" : 2.642658761824123
+        },
+        "rotation" : {
+          "radians" : 0.040562671415580655
+        }
+      },
+      "curvature" : -0.039940377095716434
+    }, {
+      "time" : 3.3123968620373043,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.207392715413668,
+          "y" : 2.6456054985607906
+        },
+        "rotation" : {
+          "radians" : 0.03763198896608686
+        }
+      },
+      "curvature" : -0.03710839155892508
+    }, {
+      "time" : 3.336527620370588,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.279737142147496,
+          "y" : 2.6482371140737087
+        },
+        "rotation" : {
+          "radians" : 0.035174735467647336
+        }
+      },
+      "curvature" : -0.02992149474512993
+    }, {
+      "time" : 3.359666591601313,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.349113313948472,
+          "y" : 2.6506150738690977
+        },
+        "rotation" : {
+          "radians" : 0.033495618460887784
+        }
+      },
+      "curvature" : -0.017400083112939065
+    }, {
+      "time" : 3.3818617651034137,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.415662353166454,
+          "y" : 2.6528189160273996
+        },
+        "rotation" : {
+          "radians" : 0.03291987664459664
+        }
+      },
+      "curvature" : 0.0013263625292214375
+    }, {
+      "time" : 3.423739530881724,
+      "velocity" : 3.0,
+      "acceleration" : -0.8175000091130344,
+      "pose" : {
+        "translation" : {
+          "x" : 4.5412228441782645,
+          "y" : 2.657095421156555
+        },
+        "rotation" : {
+          "radians" : 0.0363729513028002
+        }
+      },
+      "curvature" : 0.05866331262890874
+    }, {
+      "time" : 3.4632152976715145,
+      "velocity" : 2.967728560289602,
+      "acceleration" : -1.882760480943012,
+      "pose" : {
+        "translation" : {
+          "x" : 4.658912906437536,
+          "y" : 2.6619545562850817
+        },
+        "rotation" : {
+          "radians" : 0.047679751894998845
+        }
+      },
+      "curvature" : 0.13592977249614052
+    }, {
+      "time" : 3.5018404054419703,
+      "velocity" : 2.8950067338072225,
+      "acceleration" : -1.9238632002001914,
+      "pose" : {
+        "translation" : {
+          "x" : 4.771954350173473,
+          "y" : 2.668388433754444
+        },
+        "rotation" : {
+          "radians" : 0.0674512871959646
+        }
+      },
+      "curvature" : 0.2106703311228447
+    }, {
+      "time" : 3.541164793474475,
+      "velocity" : 2.8193519908010933,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 4.883949141742278,
+          "y" : 2.677400863588673
+        },
+        "rotation" : {
+          "radians" : 0.09396342350209999
+        }
+      },
+      "curvature" : 0.2540441775959243
+    }, {
+      "time" : 3.583307920417398,
+      "velocity" : 2.6507794830294023,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.998532040779537,
+          "y" : 2.689914865659375
+        },
+        "rotation" : {
+          "radians" : 0.12355864176280444
+        }
+      },
+      "curvature" : 0.25210444079517663
+    }, {
+      "time" : 3.630910520336883,
+      "velocity" : 2.460369083351462,
+      "acceleration" : -3.999999999999993,
+      "pose" : {
+        "translation" : {
+          "x" : 5.119023237352621,
+          "y" : 2.706680181850743
+        },
+        "rotation" : {
+          "radians" : 0.1521781406035511
+        }
+      },
+      "curvature" : 0.21456009552095912
+    }, {
+      "time" : 3.6575544532910382,
+      "velocity" : 2.3537933515348413,
+      "acceleration" : -3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 5.182350507669064,
+          "y" : 2.7168212357026604
+        },
+        "rotation" : {
+          "radians" : 0.16512628632297052
+        }
+      },
+      "curvature" : 0.18899817754880754
+    }, {
+      "time" : 3.6866111335997718,
+      "velocity" : 2.237566630299908,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.2480809891130775,
+          "y" : 2.728180788224563
+        },
+        "rotation" : {
+          "radians" : 0.1768351792035555
+        }
+      },
+      "curvature" : 0.16220610323939916
+    }, {
+      "time" : 3.718563991190063,
+      "velocity" : 2.109755199938744,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.3163865477339485,
+          "y" : 2.740762636728995
+        },
+        "rotation" : {
+          "radians" : 0.1871757577997712
+        }
+      },
+      "curvature" : 0.13593088502288128
+    }, {
+      "time" : 3.754021980661885,
+      "velocity" : 1.967923242051456,
+      "acceleration" : -4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 5.387354258449022,
+          "y" : 2.7545424071852267
+        },
+        "rotation" : {
+          "radians" : 0.19609326514864456
+        }
+      },
+      "curvature" : 0.11126066622925063
+    }, {
+      "time" : 3.7938015783178165,
+      "velocity" : 1.8088048514277302,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.460975549954718,
+          "y" : 2.7694646639744107
+        },
+        "rotation" : {
+          "radians" : 0.20358486920868646
+        }
+      },
+      "curvature" : 0.08871019500715766
+    }, {
+      "time" : 3.8390908801882153,
+      "velocity" : 1.627647643946134,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 5.537135349637538,
+          "y" : 2.7854400196447386
+        },
+        "rotation" : {
+          "radians" : 0.20967774359389213
+        }
+      },
+      "curvature" : 0.06834921616165193
+    }, {
+      "time" : 3.8918213921686315,
+      "velocity" : 1.4167255960244693,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.615601228485083,
+          "y" : 2.8023422446665975
+        },
+        "rotation" : {
+          "radians" : 0.21440925415138234
+        }
+      },
+      "curvature" : 0.049921154171936793
+    }, {
+      "time" : 3.955692162693215,
+      "velocity" : 1.1612425139261353,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.696012545997064,
+          "y" : 2.8200053771877265
+        },
+        "rotation" : {
+          "radians" : 0.21780924678420455
+        }
+      },
+      "curvature" : 0.03292187698251918
+    }, {
+      "time" : 4.040210129462593,
+      "velocity" : 0.8231706468486228,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.777869595096313,
+          "y" : 2.838220832788373
+        },
+        "rotation" : {
+          "radians" : 0.21988311898862645
+        }
+      },
+      "curvature" : 0.016620346183280694
+    }, {
+      "time" : 4.246002791174749,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.860522747039795,
+          "y" : 2.85673451423645
+        },
+        "rotation" : {
+          "radians" : 0.22059308726779464
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 4.100522994995117, -3.486077070236206, 0.0 ],
+      "y" : [ -0.7109201550483704, 2.165250778198242, 0.0 ]
+    }, {
+      "x" : [ 1.8818800449371338, -0.19111382961273193, 0.0 ],
+      "y" : [ 1.2418794631958008, 1.7965017557144165, 0.0 ]
+    }, {
+      "x" : [ 3.6274187564849854, 2.828535318374634, 0.0 ],
+      "y" : [ 2.617924213409424, 0.1528944969177246, 0.0 ]
+    }, {
+      "x" : [ 5.860522747039795, 2.649440288543701, 0.0 ],
+      "y" : [ 2.85673451423645, 0.5941164493560791, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.584307420515759
+      }
+    }, {
+      "time" : 1.699999962002039,
+      "rotation" : {
+        "radians" : 2.7676508674151563
+      }
+    }, {
+      "time" : 3.1199999302625656,
+      "rotation" : {
+        "radians" : 0.4494112193892231
+      }
+    }, {
+      "time" : 4.159999907016754,
+      "rotation" : {
+        "radians" : 0.46364803834258117
+      }
+    } ],
+    "reversed" : false,
+    "color" : 286.3529,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  } ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/autos/red/5ball.json b/src/main/deploy/autos/red/5ball.json
index de3ff4dc..5d82b260 100644
--- a/src/main/deploy/autos/red/5ball.json
+++ b/src/main/deploy/autos/red/5ball.json
@@ -212,7 +212,7 @@
     "constraints" : [ ]
   }, {
     "type" : "script",
-    "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 7",
+    "script" : "# Shoot the first 2 balls\n#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 5",
     "closed" : false,
     "valid" : true,
     "sendableScript" : {
@@ -220,7 +220,7 @@
       "delay" : 0.0,
       "commands" : [ {
         "methodName" : "frc.subsystem.VisionManager.shootBalls",
-        "args" : [ "7" ],
+        "args" : [ "5" ],
         "argTypes" : [ "double" ],
         "reflection" : true
       } ]
@@ -250,452 +250,466 @@
     "states" : [ {
       "time" : 0.0,
       "velocity" : 0.0,
-      "acceleration" : 4.0,
+      "acceleration" : 3.999999999999999,
       "pose" : {
         "translation" : {
           "x" : 7.626592636108398,
           "y" : -3.196845054626465
         },
         "rotation" : {
-          "radians" : 2.70041390445855
+          "radians" : -3.128825218368557
         }
       },
-      "curvature" : -0.0
+      "curvature" : 0.0
     }, {
-      "time" : 0.23678198316332957,
-      "velocity" : 0.9471279326533183,
+      "time" : 0.18074836042161999,
+      "velocity" : 0.7229934416864798,
       "acceleration" : 3.9999999999999973,
       "pose" : {
         "translation" : {
-          "x" : 7.5253763288478694,
-          "y" : -3.1485883100490355
+          "x" : 7.561256032039637,
+          "y" : -3.197505262483041
         },
         "rotation" : {
-          "radians" : 2.6900449899598926
+          "radians" : -3.136668850289797
         }
       },
-      "curvature" : -0.15715157236253124
+      "curvature" : -0.2308900234918559
     }, {
-      "time" : 0.3409986064846093,
-      "velocity" : 1.363994425938437,
-      "acceleration" : 4.000000000000002,
+      "time" : 0.2562629294282206,
+      "velocity" : 1.025051717712882,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 7.417564729417791,
-          "y" : -3.094925222234451
+          "x" : 7.495255342415476,
+          "y" : -3.1971908724897276
         },
         "rotation" : {
-          "radians" : 2.6692426474617976
+          "radians" : 3.1252538533846033
         }
       },
-      "curvature" : -0.16963195910628098
+      "curvature" : -0.4025150795851139
     }, {
-      "time" : 0.3857824438824356,
-      "velocity" : 1.5431297755297422,
-      "acceleration" : 3.9999999999999933,
+      "time" : 0.3151027670199877,
+      "velocity" : 1.2604110680799503,
+      "acceleration" : 3.999999999999994,
       "pose" : {
         "translation" : {
-          "x" : 7.359754048377923,
-          "y" : -3.065001553835387
+          "x" : 7.428050091650221,
+          "y" : -3.195088412615668
         },
         "rotation" : {
-          "radians" : 2.658923020471842
+          "radians" : 3.0941603378329714
         }
       },
-      "curvature" : -0.14581550944396268
+      "curvature" : -0.5118276847264331
     }, {
-      "time" : 0.4281176659650556,
-      "velocity" : 1.712470663860222,
-      "acceleration" : 4.0,
+      "time" : 0.365754563843708,
+      "velocity" : 1.4630182553748314,
+      "acceleration" : 3.999999999999994,
       "pose" : {
         "translation" : {
-          "x" : 7.298863693187286,
-          "y" : -3.0327307414841016
+          "x" : 7.359226277680136,
+          "y" : -3.19054991419398
         },
         "rotation" : {
-          "radians" : 2.6499291206925197
+          "radians" : 3.0567199098033253
         }
       },
-      "curvature" : -0.11488482457954703
+      "curvature" : -0.5650059367626722
     }, {
-      "time" : 0.4687787361650137,
-      "velocity" : 1.8751149446600544,
-      "acceleration" : 4.000000000000009,
+      "time" : 0.41151249073481067,
+      "velocity" : 1.6460499629392418,
+      "acceleration" : 3.999999999999991,
       "pose" : {
         "translation" : {
-          "x" : 7.234696646951008,
-          "y" : -2.998054031586136
+          "x" : 7.288487160873956,
+          "y" : -3.183081614639285
         },
         "rotation" : {
-          "radians" : 2.642744382686732
+          "radians" : 3.0159966655848844
         }
       },
-      "curvature" : -0.08249583112954426
+      "curvature" : -0.5735194202335906
     }, {
-      "time" : 0.5081929441867301,
-      "velocity" : 2.0327717767469204,
-      "acceleration" : 3.9999999999999947,
+      "time" : 0.45404678811194626,
+      "velocity" : 1.8161871524477837,
+      "acceleration" : 3.999999999999997,
       "pose" : {
         "translation" : {
-          "x" : 7.167170996312052,
-          "y" : -2.9610228459350765
+          "x" : 7.215644052943389,
+          "y" : -3.172332660165239
         },
         "rotation" : {
-          "radians" : 2.637607535762462
+          "radians" : 2.974462343798371
         }
       },
-      "curvature" : -0.05155148354596137
+      "curvature" : -0.5505200761933615
     }, {
-      "time" : 0.5465891606616753,
-      "velocity" : 2.186356642646701,
-      "acceleration" : 3.9999999999999893,
+      "time" : 0.49431508345730446,
+      "velocity" : 1.9772603338292163,
+      "acceleration" : 3.4442482007392234,
       "pose" : {
         "translation" : {
-          "x" : 7.096308985695558,
-          "y" : -2.92178689978671
+          "x" : 7.140607105853633,
+          "y" : -3.158083808502063
         },
         "rotation" : {
-          "radians" : 2.6346096400057593
+          "radians" : 2.933950997371189
         }
       },
-      "curvature" : -0.023151408617227538
+      "curvature" : -0.5081482982157391
     }, {
-      "time" : 0.5840763060621219,
-      "velocity" : 2.336305224248487,
-      "acceleration" : 4.0,
+      "time" : 0.533094323956189,
+      "velocity" : 2.110825663143533,
+      "acceleration" : 3.7197245506146066,
       "pose" : {
         "translation" : {
-          "x" : 7.0222260715531775,
-          "y" : -2.8805823199331826
+          "x" : 7.063376100733876,
+          "y" : -3.140236131614074
         },
         "rotation" : {
-          "radians" : 2.6337649653389965
+          "radians" : 2.8957084348464917
         }
       },
-      "curvature" : 0.0026288919328956535
+      "curvature" : -0.45605683856028184
     }, {
-      "time" : 0.6206887373405835,
-      "velocity" : 2.4827549493623335,
-      "acceleration" : 4.0,
+      "time" : 0.5707801304586655,
+      "velocity" : 2.2510064828005065,
+      "acceleration" : 4.000000000000005,
       "pose" : {
         "translation" : {
-          "x" : 6.945119976607415,
-          "y" : -2.837719762777155
+          "x" : 6.984031236787814,
+          "y" : -3.1187997184172147
         },
         "rotation" : {
-          "radians" : 2.6350579923694193
+          "radians" : 2.860495559941412
         }
       },
-      "curvature" : 0.02622781759990982
+      "curvature" : -0.40102385925601364
     }, {
-      "time" : 0.6564142795761782,
-      "velocity" : 2.625657118304712,
-      "acceleration" : 3.999999999999995,
+      "time" : 0.6073691854310432,
+      "velocity" : 2.3973627026900175,
+      "acceleration" : 3.99999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 6.902723920204153,
+          "y" : -3.093882377496584
+        },
+        "rotation" : {
+          "radians" : 2.8287087580304324
+        }
+      },
+      "curvature" : -0.3472627622064644
+    }, {
+      "time" : 0.6429036781342928,
+      "velocity" : 2.5395006735030154,
+      "acceleration" : 4.000000000000005,
       "pose" : {
         "translation" : {
-          "x" : 6.865259744095965,
-          "y" : -2.7935725324059604
+          "x" : 6.81966755306712,
+          "y" : -3.065678339823968
         },
         "rotation" : {
-          "radians" : 2.638472613930082
+          "radians" : 2.8004923774748782
         }
       },
-      "curvature" : 0.048307452945012146
+      "curvature" : -0.29702381463813854
     }, {
-      "time" : 0.6912124020261066,
-      "velocity" : 2.764849608104426,
-      "acceleration" : 2.5754078048514004,
+      "time" : 0.6774510929226875,
+      "velocity" : 2.677690332656594,
+      "acceleration" : 4.000000000000004,
       "pose" : {
         "translation" : {
-          "x" : 6.782974792016049,
-          "y" : -2.74856469866576
+          "x" : 6.7351283222669736,
+          "y" : -3.034456961475371
         },
         "rotation" : {
-          "radians" : 2.644009420306148
+          "radians" : 2.775831102570153
         }
       },
-      "curvature" : 0.06960805639848362
+      "curvature" : -0.2512121840579067
     }, {
-      "time" : 0.725312002060465,
-      "velocity" : 2.8526699841752237,
-      "acceleration" : -4.000000000000009,
+      "time" : 0.7110320540420592,
+      "velocity" : 2.8120141771340808,
+      "acceleration" : -3.2029286867045355,
       "pose" : {
         "translation" : {
-          "x" : 6.698643967368753,
-          "y" : -2.7031592152357007
+          "x" : 6.64941598841051,
+          "y" : -3.0005514263485438
         },
         "rotation" : {
-          "radians" : 2.6516955032387415
+          "radians" : 2.7546193861322052
         }
       },
-      "curvature" : 0.09088679864226243
+      "curvature" : -0.20989196042644145
     }, {
-      "time" : 0.7602301806429926,
-      "velocity" : 2.7129972698451126,
+      "time" : 0.7450512183156593,
+      "velocity" : 2.7030532199844526,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 6.6126846004033695,
-          "y" : -2.657846037702072
+          "x" : 6.5628746747315745,
+          "y" : -2.964347448880517
         },
         "rotation" : {
-          "radians" : 2.66158969110175
+          "radians" : 2.7367102690047114
         }
       },
-      "curvature" : 0.11290092691377968
+      "curvature" : -0.17264507479778918
     }, {
-      "time" : 0.7973483461427653,
-      "velocity" : 2.564524607846022,
-      "acceleration" : -3.9999999999999956,
+      "time" : 0.781148946516508,
+      "velocity" : 2.558662307181058,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 6.5255415588617325,
-          "y" : -2.6131302416324615
+          "x" : 6.475873656001568,
+          "y" : -2.926271976765129
         },
         "rotation" : {
-          "radians" : 2.6737848783502094
+          "radians" : 2.721947850294995
         }
       },
-      "curvature" : 0.1364110321450652
+      "curvature" : -0.13880277069969946
     }, {
-      "time" : 0.8368127385519012,
-      "velocity" : 2.4066670382094784,
+      "time" : 0.819677131050652,
+      "velocity" : 2.404549569044482,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 6.437676302222556,
-          "y" : -2.5695201406499137
+          "x" : 6.388798147439957,
+          "y" : -2.886781893670559
         },
         "rotation" : {
-          "radians" : 2.6884081376830955
+          "radians" : 2.7101877381984085
         }
       },
-      "curvature" : 0.162189062203917
+      "curvature" : -0.10758244075317105
     }, {
-      "time" : 0.8788429529768294,
-      "velocity" : 2.2385461805097657,
-      "acceleration" : -3.9999999999999956,
+      "time" : 0.8608963508797474,
+      "velocity" : 2.2396726897281005,
+      "acceleration" : -4.000000000000004,
       "pose" : {
         "translation" : {
-          "x" : 6.349555935945773,
-          "y" : -2.527515404507085
+          "x" : 6.3020400936247825,
+          "y" : -2.8463527219568547
         },
         "rotation" : {
-          "radians" : 2.7056184913490395
+          "radians" : 2.701309038995795
         }
       },
-      "curvature" : 0.19101923638415347
+      "curvature" : -0.07816156796989873
     }, {
-      "time" : 0.9237789286790171,
-      "velocity" : 2.0588022777010146,
-      "acceleration" : -4.0,
+      "time" : 0.9051855357994181,
+      "velocity" : 2.0625159500494177,
+      "acceleration" : -4.000000000000004,
       "pose" : {
         "translation" : {
-          "x" : 6.261642265716873,
-          "y" : -2.487595177160401
+          "x" : 6.2159889574031695,
+          "y" : -2.805467325393465
         },
         "rotation" : {
-          "radians" : 2.7256014363493466
+          "radians" : 2.695220423456694
         }
       },
-      "curvature" : 0.2236791506698763
+      "curvature" : -0.049714314498870184
     }, {
-      "time" : 0.9721645748556083,
-      "velocity" : 1.86525969299465,
-      "acceleration" : -3.999999999999998,
+      "time" : 0.9531262186792658,
+      "velocity" : 1.8707532185300264,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 6.1743808516912395,
-          "y" : -2.4502061948442133
+          "x" : 6.131022508801834,
+          "y" : -2.76460461187677
         },
         "rotation" : {
-          "radians" : 2.748558448602262
+          "radians" : 2.6918618037119115
         }
       },
-      "curvature" : 0.2608836831433858
+      "curvature" : -0.021430491946737754
     }, {
-      "time" : 1.02491166750831,
-      "velocity" : 1.6542713223838432,
+      "time" : 1.0056683951523762,
+      "velocity" : 1.6605845126375849,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 6.088190062738491,
-          "y" : -2.415750904144957
+          "x" : 6.047497613937594,
+          "y" : -2.72422823614761
         },
         "rotation" : {
-          "radians" : 2.774688654056905
+          "radians" : 2.6912022196748593
         }
       },
-      "curvature" : 0.30316623147805977
+      "curvature" : 0.007466521997725307
     }, {
-      "time" : 1.083666813203764,
-      "velocity" : 1.4192507396020277,
-      "acceleration" : -3.999999999999996,
+      "time" : 1.0645039443355842,
+      "velocity" : 1.425242315904753,
+      "acceleration" : -3.9999999999999987,
       "pose" : {
         "translation" : {
-          "x" : 6.003450130686815,
-          "y" : -2.384575580075307
+          "x" : 5.965741023927876,
+          "y" : -2.684775302508818
         },
         "rotation" : {
-          "radians" : 2.804158659356016
+          "radians" : 2.6932336344067656
         }
       },
-      "curvature" : 0.3506617141264975
+      "curvature" : 0.03768607383081342
     }, {
-      "time" : 1.1518177007752968,
-      "velocity" : 1.1466471893158965,
+      "time" : 1.133097575805044,
+      "velocity" : 1.1508677900269133,
       "acceleration" : -3.9999999999999987,
       "pose" : {
         "translation" : {
-          "x" : 5.92049220456731,
-          "y" : -2.356958444148333
+          "x" : 5.886040163801226,
+          "y" : -2.6466450675427495
         },
         "rotation" : {
-          "radians" : 2.8370553812686623
+          "radians" : 2.697959401678686
         }
       },
-      "curvature" : 0.40274814478615856
+      "curvature" : 0.06979988927808267
     }, {
-      "time" : 1.2384794981042708,
+      "time" : 1.2208145233117724,
       "velocity" : 0.8,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.839587404858321,
-          "y" : -2.3330977824516594
+          "x" : 5.808633921407818,
+          "y" : -2.610187642828812
         },
         "rotation" : {
-          "radians" : 2.873316167854496
+          "radians" : 2.705375145901104
         }
       },
-      "curvature" : 0.4575095057609104
+      "curvature" : 0.10410688222029436
     }, {
-      "time" : 1.3399219962409736,
+      "time" : 1.323926104530657,
       "velocity" : 0.8,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.7609358777297786,
-          "y" : -2.3131000637216204
+          "x" : 5.733703436329961,
+          "y" : -2.5756926976609975
         },
         "rotation" : {
-          "radians" : 2.9126317023886243
+          "radians" : 2.715438727807114
         }
       },
-      "curvature" : 0.5110229996682454
+      "curvature" : 0.14039632008460795
     }, {
-      "time" : 1.4373809969681062,
+      "time" : 1.422963534703475,
       "velocity" : 0.8,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.684655849287537,
-          "y" : -2.2969680574174163
+          "x" : 5.66136288879261,
+          "y" : -2.5433781617654105
         },
         "rotation" : {
-          "radians" : 2.954321888441968
+          "radians" : 2.7280251266126854
         }
       },
-      "curvature" : 0.5565765979019635
+      "curvature" : 0.17756966797704812
     }, {
-      "time" : 1.5310223088557995,
+      "time" : 1.5178302228662353,
       "velocity" : 0.8,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.610772679817714,
-          "y" : -2.2845889517952713
+          "x" : 5.591650288573874,
+          "y" : -2.5133789280178007
         },
         "rotation" : {
-          "radians" : 2.9971949967281697
+          "radians" : 2.742862160518646
         }
       },
-      "curvature" : 0.5841027262519725
+      "curvature" : 0.21310100879858962
     }, {
-      "time" : 1.621162213950032,
+      "time" : 1.6085811250451547,
       "velocity" : 0.8,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.539207918031025,
-          "y" : -2.27572247198259
+          "x" : 5.524518263915525,
+          "y" : -2.4857355551610922
         },
         "rotation" : {
-          "radians" : 3.039416140548233
+          "radians" : 2.7594454148032215
         }
       },
-      "curvature" : 0.5803251759571693
+      "curvature" : 0.2423895877411266
     }, {
-      "time" : 1.7082570400969352,
+      "time" : 1.6954358105818255,
       "velocity" : 0.8,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.469768355307124,
-          "y" : -2.2699889980521135
+          "x" : 5.459824850433506,
+          "y" : -2.4603829705229145
         },
         "rotation" : {
-          "radians" : 3.0784289185307894
+          "radians" : 2.7769377831485036
         }
       },
-      "curvature" : 0.530208726702976
+      "curvature" : 0.2582273725929391
     }, {
-      "time" : 1.792889195170949,
+      "time" : 1.7787893070277847,
       "velocity" : 0.8,
-      "acceleration" : -0.8727668696952652,
+      "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 5.402135079938944,
-          "y" : -2.2668576830960774
+          "x" : 5.3973242800284424,
+          "y" : -2.437139172733133
         },
         "rotation" : {
-          "radians" : 3.110985106966813
+          "radians" : 2.794073008855231
         }
       },
-      "curvature" : 0.42007567627926556
+      "curvature" : 0.2508829468700981
     }, {
-      "time" : 1.879884801763329,
-      "velocity" : 0.7240731167571277,
-      "acceleration" : -4.0,
+      "time" : 1.8592209910556299,
+      "velocity" : 0.8,
+      "acceleration" : -2.574202589173992,
       "pose" : {
         "translation" : {
-          "x" : 5.335852531377029,
-          "y" : -2.2656345713003674
+          "x" : 5.3366577697961475,
+          "y" : -2.4156939344413786
         },
         "rotation" : {
-          "radians" : 3.133332651308909
+          "radians" : 2.8091035777764453
         }
       },
-      "curvature" : 0.24218749049492908
+      "curvature" : 0.20957902888612367
     }, {
-      "time" : 2.060903080952611,
+      "time" : 2.1699968419402733,
       "velocity" : 0.0,
-      "acceleration" : -4.0,
+      "acceleration" : -2.574202589173992,
       "pose" : {
         "translation" : {
-          "x" : 5.270317554473877,
-          "y" : -2.2654507160186768
+          "x" : 5.218771457672119,
+          "y" : -2.376249313354492
         },
         "rotation" : {
-          "radians" : 3.141592653589793
+          "radians" : 2.823947779781842
         }
       },
       "curvature" : -0.0
     } ],
     "pointList" : [ {
-      "x" : [ 7.626592636108398, -1.5994892120361328, 0.0 ],
-      "y" : [ -3.196845054626465, 0.7553129196166992, 0.0 ]
+      "x" : [ 7.626592636108398, -2.086981773376465, 0.0 ],
+      "y" : [ -3.196845054626465, -0.026646852493286133, 0.0 ]
     }, {
-      "x" : [ 5.270317554473877, -2.0928425788879395, 0.0 ],
-      "y" : [ -2.2654507160186768, 0.0, 0.0 ]
+      "x" : [ 5.218771457672119, -1.8701248168945312, 0.0 ],
+      "y" : [ -2.376249313354492, 0.6148552894592285, 0.0 ]
     } ],
     "rotations" : [ {
       "time" : 0.0,
       "rotation" : {
-        "radians" : -3.1179661408942683
+        "radians" : 2.9608083881863267
       }
     }, {
-      "time" : 1.9699999559670687,
+      "time" : 2.059999953955412,
       "rotation" : {
         "radians" : -3.1066860685499065
       }
@@ -722,7 +736,7 @@
     } ]
   }, {
     "type" : "script",
-    "script" : "VisionManager.shootBalls 7",
+    "script" : "VisionManager.shootBalls 4.5",
     "closed" : false,
     "valid" : true,
     "sendableScript" : {
@@ -730,7 +744,7 @@
       "delay" : 0.0,
       "commands" : [ {
         "methodName" : "frc.subsystem.VisionManager.shootBalls",
-        "args" : [ "7" ],
+        "args" : [ "4.5" ],
         "argTypes" : [ "double" ],
         "reflection" : true
       } ]
@@ -760,711 +774,683 @@
     "states" : [ {
       "time" : 0.0,
       "velocity" : 0.0,
-      "acceleration" : 4.000000000000001,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 5.270317554473877,
-          "y" : -2.2654507160186768
+          "x" : 5.218771457672119,
+          "y" : -2.376249313354492
         },
         "rotation" : {
-          "radians" : -3.134549682568558
+          "radians" : 2.9473819457269834
         }
       },
-      "curvature" : 0.0
+      "curvature" : -0.0
     }, {
-      "time" : 0.24842369994777175,
-      "velocity" : 0.9936947997910872,
-      "acceleration" : 3.9999999999999996,
+      "time" : 0.2258499088780425,
+      "velocity" : 0.90339963551217,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 5.1468907883983235,
-          "y" : -2.2661361676086145
+          "x" : 5.11868232863435,
+          "y" : -2.356513444916345
         },
         "rotation" : {
-          "radians" : -3.13891982536029
+          "radians" : 2.9464389269674878
         }
       },
-      "curvature" : -0.06757119451203722
+      "curvature" : -0.002138170805929468
     }, {
-      "time" : 0.3520110014258871,
-      "velocity" : 1.4080440057035486,
+      "time" : 0.28397149126868465,
+      "velocity" : 1.1358859650747386,
       "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 5.022496396452425,
-          "y" : -2.2658095045383106
+          "x" : 5.060542505813004,
+          "y" : -2.345028926573832
         },
         "rotation" : {
-          "radians" : 3.1326792429030137
+          "radians" : 2.9468943393418883
         }
       },
-      "curvature" : -0.11527041271105326
+      "curvature" : 0.01712904542764913
     }, {
-      "time" : 0.43240960818409263,
-      "velocity" : 1.7296384327363707,
-      "acceleration" : 3.9999999999999964,
+      "time" : 0.33858988144624014,
+      "velocity" : 1.3543595257849605,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 4.896381715565369,
-          "y" : -2.2636806035667334
+          "x" : 4.993810497137019,
+          "y" : -2.3319240934215486
         },
         "rotation" : {
-          "radians" : 3.116151981669022
+          "radians" : 2.9486470176545607
         }
       },
-      "curvature" : -0.14367915368249928
+      "curvature" : 0.03310799771885568
     }, {
-      "time" : 0.46790088797956003,
-      "velocity" : 1.8716035519182401,
-      "acceleration" : 3.9999999999999964,
+      "time" : 0.3922352671630025,
+      "velocity" : 1.56894106865201,
+      "acceleration" : 3.999999999999995,
       "pose" : {
         "translation" : {
-          "x" : 4.83250436842793,
-          "y" : -2.2617556996466606
+          "x" : 4.9168329080531805,
+          "y" : -2.317000843407186
         },
         "rotation" : {
-          "radians" : 3.1067000622999905
+          "radians" : 2.9516944809829795
         }
       },
-      "curvature" : -0.15145737343185342
+      "curvature" : 0.04352423972613848
     }, {
-      "time" : 0.5012011234061037,
-      "velocity" : 2.0048044936244147,
-      "acceleration" : 3.9999999999999796,
+      "time" : 0.445877887656658,
+      "velocity" : 1.7835115506266317,
+      "acceleration" : 3.9999999999999956,
       "pose" : {
         "translation" : {
-          "x" : 4.768012917062151,
-          "y" : -2.2591854071142734
+          "x" : 4.828497262972633,
+          "y" : -2.3002106203348376
         },
         "rotation" : {
-          "radians" : 3.096774078649599
+          "radians" : 2.9559185031716857
         }
       },
-      "curvature" : -0.15553616018271685
+      "curvature" : 0.04975547748132501
     }, {
-      "time" : 0.5327416181740833,
-      "velocity" : 2.1309664726963327,
-      "acceleration" : 3.9999999999999867,
+      "time" : 0.499814290021222,
+      "velocity" : 1.9992571600848876,
+      "acceleration" : 4.0000000000000036,
       "pose" : {
         "translation" : {
-          "x" : 4.702871997726714,
-          "y" : -2.2559316347583964
+          "x" : 4.728187360616829,
+          "y" : -2.281639807416468
         },
         "rotation" : {
-          "radians" : 3.0865855244923694
+          "radians" : 2.961208234474397
         }
       },
-      "curvature" : -0.15638926647036192
+      "curvature" : 0.053603892320815395
     }, {
-      "time" : 0.562832446555475,
-      "velocity" : 2.2513297862218993,
-      "acceleration" : 4.0,
+      "time" : 0.5540160403320278,
+      "velocity" : 2.2160641613281107,
+      "acceleration" : 4.0000000000000036,
       "pose" : {
         "translation" : {
-          "x" : 4.637057769139531,
-          "y" : -2.251968205787257
+          "x" : 4.615738629363477,
+          "y" : -2.2614951208233833
         },
         "rotation" : {
-          "radians" : 3.0763231525028454
+          "radians" : 2.967499617187574
         }
       },
-      "curvature" : -0.15448651685997875
+      "curvature" : 0.056398078240657186
     }, {
-      "time" : 0.5917035370956308,
-      "velocity" : 2.3668141483825225,
+      "time" : 0.6082934243307945,
+      "velocity" : 2.4331736973231775,
       "acceleration" : 4.000000000000006,
       "pose" : {
         "translation" : {
-          "x" : 4.570557373805121,
-          "y" : -2.2472803041573766
+          "x" : 4.4913934825924855,
+          "y" : -2.240089003237699
         },
         "rotation" : {
-          "radians" : 3.066153211117251
+          "radians" : 2.974780294083102
         }
       },
-      "curvature" : -0.15027440895945235
+      "curvature" : 0.0589859321752288
     }, {
-      "time" : 0.6195292935585188,
-      "velocity" : 2.4781171742340744,
-      "acceleration" : 3.9999999999999933,
+      "time" : 0.6353787681419027,
+      "velocity" : 2.5415150725676106,
+      "acceleration" : 3.999999999999994,
       "pose" : {
         "translation" : {
-          "x" : 4.50336839934198,
-          "y" : -2.2418639209024604
+          "x" : 4.424935805760302,
+          "y" : -2.2290358611979
         },
         "rotation" : {
-          "radians" : 3.0562204023412582
+          "radians" : 2.9788008074550336
         }
       },
-      "curvature" : -0.14416258965427287
+      "curvature" : 0.06037816383649394
     }, {
-      "time" : 0.6464441948512846,
-      "velocity" : 2.5857767794051374,
-      "acceleration" : 4.000000000000006,
+      "time" : 0.6623797970275374,
+      "velocity" : 2.649519188110149,
+      "acceleration" : 3.999999999999982,
       "pose" : {
         "translation" : {
-          "x" : 4.435498339809959,
-          "y" : -2.235725300462292
+          "x" : 4.355756674031909,
+          "y" : -2.217825017403811
         },
         "rotation" : {
-          "radians" : 3.0466493652938835
+          "radians" : 2.983085127613897
         }
       },
-      "curvature" : -0.13651545432943565
+      "curvature" : 0.061904813840425225
     }, {
-      "time" : 0.6725531388457419,
-      "velocity" : 2.6902125553829666,
-      "acceleration" : 3.999999999999994,
+      "time" : 0.6892585612750203,
+      "velocity" : 2.75703424510008,
+      "acceleration" : 4.000000000000018,
       "pose" : {
         "translation" : {
-          "x" : 4.3669640570376345,
-          "y" : -2.228880387011621
+          "x" : 4.2839809727891565,
+          "y" : -2.206518758337751
         },
         "rotation" : {
-          "radians" : 3.037546511729907
+          "radians" : 2.987644244198006
         }
       },
-      "curvature" : -0.12764795403681967
+      "curvature" : 0.06360865823678644
     }, {
-      "time" : 0.6979385669929336,
-      "velocity" : 2.7917542679717333,
+      "time" : 0.7159768045103969,
+      "velocity" : 2.863907218041587,
       "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 4.297791241949682,
-          "y" : -2.221354270789056
+          "x" : 4.209750653103896,
+          "y" : -2.1951832396798636
         },
         "rotation" : {
-          "radians" : 3.029002066155314
+          "radians" : 2.992491671784849
         }
       },
-      "curvature" : -0.1178246535056758
+      "curvature" : 0.06552874485970991
     }, {
-      "time" : 0.7226655334792509,
-      "velocity" : 2.8906621339170027,
-      "acceleration" : 3.9999999999999876,
+      "time" : 0.742496452777877,
+      "velocity" : 2.9699858111115076,
+      "acceleration" : 1.127748575111186,
       "pose" : {
         "translation" : {
-          "x" : 4.2280138758942485,
-          "y" : -2.213180634425953
+          "x" : 4.133223336592542,
+          "y" : -2.183888029856604
         },
         "rotation" : {
-          "radians" : 3.021092191458384
+          "radians" : 2.997643463480912
         }
       },
-      "curvature" : -0.10726113181941763
+      "curvature" : 0.06770258788043935
     }, {
-      "time" : 0.7467854146298516,
-      "velocity" : 2.987141658519405,
-      "acceleration" : 0.5386070895034664,
+      "time" : 0.7691107084292603,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 4.157673691970331,
-          "y" : -2.2044011992753108
+          "x" : 4.054570920270635,
+          "y" : -2.172705653589219
         },
         "rotation" : {
-          "radians" : 3.0138811087611876
+          "radians" : 3.0031182727570744
         }
       },
-      "curvature" : -0.09612692548068995
+      "curvature" : 0.07016799487252837
     }, {
-      "time" : 0.7706587384890842,
+      "time" : 0.7962237823292616,
       "velocity" : 3.0,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 4.086819636355145,
-          "y" : -2.1950651717406586
+          "x" : 3.9739781814073973,
+          "y" : -2.161711135442232
         },
         "rotation" : {
-          "radians" : 3.0074231456653506
+          "radians" : 3.0089374623838587
         }
       },
-      "curvature" : -0.08454935140914073
+      "curvature" : 0.0729646488132685
     }, {
-      "time" : 0.7946545748664725,
+      "time" : 0.8239014360111758,
       "velocity" : 3.0,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 4.0155073296314985,
-          "y" : -2.1852286896049478
+          "x" : 3.8916413823802998,
+          "y" : -2.150981543371927
         },
         "rotation" : {
-          "radians" : 3.001764669123353
+          "radians" : 3.015125259707474
         }
       },
-      "curvature" : -0.0726176921950453
+      "curvature" : 0.07613554688814887
     }, {
-      "time" : 0.818801615050494,
+      "time" : 0.8520731360071213,
       "velocity" : 3.0,
-      "acceleration" : -1.2196893018554146E-14,
+      "acceleration" : -1.035442254722702E-14,
       "pose" : {
         "translation" : {
-          "x" : 3.943798528115167,
-          "y" : -2.1749542683594427
+          "x" : 3.8077668755296186,
+          "y" : -2.14059553227483
         },
         "rotation" : {
-          "radians" : 2.996945877314582
+          "radians" : 3.0217089580997
         }
       },
-      "curvature" : -0.06038736183252478
+      "curvature" : 0.0797283808237485
     }, {
-      "time" : 0.843074966696934,
+      "time" : 0.8806656984492115,
       "velocity" : 2.9999999999999996,
-      "acceleration" : 1.2147236114840852E-14,
-      "pose" : {
-        "translation" : {
-          "x" : 3.8717605851822676,
-          "y" : -2.1643102475326135
-        },
-        "rotation" : {
-          "radians" : 2.9930024391077152
-        }
-      },
-      "curvature" : -0.04788378801169793
-    }, {
-      "time" : 0.8674475458115909,
-      "velocity" : 3.0,
-      "acceleration" : 0.0,
+      "acceleration" : 1.0230783524543899E-14,
       "pose" : {
         "translation" : {
-          "x" : 3.799465912596631,
-          "y" : -2.1533702370190255
+          "x" : 3.7225697080129976,
+          "y" : -2.130632887536194
         },
         "rotation" : {
-          "radians" : 2.9899669802209736
+          "radians" : 3.02871916503608
         }
       },
-      "curvature" : -0.03510584306122642
+      "curvature" : 0.08379693042086887
     }, {
-      "time" : 0.8918903193227681,
+      "time" : 0.9096038020190037,
       "velocity" : 3.0,
       "acceleration" : 0.0,
       "pose" : {
         "translation" : {
-          "x" : 3.7269914418371743,
-          "y" : -2.14221256340823
+          "x" : 3.636272226660009,
+          "y" : -2.1211740685784832
         },
         "rotation" : {
-          "radians" : 2.987870422494782
+          "radians" : 3.036190097790499
         }
       },
-      "curvature" : -0.022028729814589355
+      "curvature" : 0.08840253262721617
     }, {
-      "time" : 0.9163725588181648,
+      "time" : 0.9388105037985717,
       "velocity" : 3.0,
-      "acceleration" : 0.0,
+      "acceleration" : -0.02866373936722982,
       "pose" : {
         "translation" : {
-          "x" : 3.6544180854252772,
-          "y" : -2.130919716313656
+          "x" : 3.549102682826714,
+          "y" : -2.1122997524098537
         },
         "rotation" : {
-          "radians" : 2.9867431872752266
+          "radians" : 3.0441599280842286
         }
       },
-      "curvature" : -0.008606283415062655
+      "curvature" : 0.09361568206062745
     }, {
-      "time" : 0.9408621043203094,
-      "velocity" : 3.0,
-      "acceleration" : 0.0,
+      "time" : 0.9682118880878174,
+      "velocity" : 2.9991572463836973,
+      "acceleration" : -0.1982391394605512,
       "pose" : {
         "translation" : {
-          "x" : 3.581830198252153,
-          "y" : -2.1195777947015015
+          "x" : 3.4612938372502233,
+          "y" : -2.104090377172639
         },
         "rotation" : {
-          "radians" : 2.9866162762632475
+          "radians" : 3.052671177092393
         }
       },
-      "curvature" : 0.005227310981712426
+      "curvature" : 0.09951781431626906
     }, {
-      "time" : 0.9653256371710356,
-      "velocity" : 3.0,
-      "acceleration" : -1.2131932184309055E-14,
+      "time" : 0.9977582138868922,
+      "velocity" : 2.9933000081830676,
+      "acceleration" : -0.22109236700729543,
       "pose" : {
         "translation" : {
-          "x" : 3.509315038906223,
-          "y" : -2.108275953219625
+          "x" : 3.3730815649032593,
+          "y" : -2.0966256856918335
         },
         "rotation" : {
-          "radians" : 2.987522243738523
+          "radians" : 3.06177116186092
         }
       },
-      "curvature" : 0.019556357371493856
+      "curvature" : 0.10620331937809746
     }, {
-      "time" : 0.9897289612859104,
-      "velocity" : 2.9999999999999996,
-      "acceleration" : 0.0,
+      "time" : 1.0273992194582766,
+      "velocity" : 2.986746608100814,
+      "acceleration" : -0.24700563523218624,
       "pose" : {
         "translation" : {
-          "x" : 3.4369622310004906,
-          "y" : -2.097105848526434
+          "x" : 3.284703459848715,
+          "y" : -2.0899842690235744
         },
         "rotation" : {
-          "radians" : 2.9894960731470905
+          "radians" : 3.071512493242552
         }
       },
-      "curvature" : 0.03448299790096692
+      "curvature" : 0.1137818273539462
     }, {
-      "time" : 1.0140372922159826,
-      "velocity" : 2.9999999999999996,
-      "acceleration" : 0.0,
+      "time" : 1.0570639837395306,
+      "velocity" : 2.9794192441555096,
+      "acceleration" : -0.276421450526931,
       "pose" : {
         "translation" : {
-          "x" : 3.3648632244999135,
-          "y" : -2.0861610856197785
+          "x" : 3.1963974400942163,
+          "y" : -2.0842431100036265
         },
         "rotation" : {
-          "radians" : 2.9925759688659737
+          "radians" : 3.0819536236657514
         }
       },
-      "curvature" : 0.050127008297662215
+      "curvature" : 0.12238080029737158
     }, {
-      "time" : 1.038215553610506,
-      "velocity" : 2.9999999999999996,
-      "acceleration" : 1.2329067798560187E-14,
+      "time" : 1.086682946461585,
+      "velocity" : 2.9712319275167762,
+      "acceleration" : -0.3098235530790123,
       "pose" : {
         "translation" : {
-          "x" : 3.2931107570487783,
-          "y" : -2.075536664165842
+          "x" : 3.108400352446682,
+          "y" : -2.079477126795865
         },
         "rotation" : {
-          "radians" : 2.9968040706287984
+          "radians" : 3.0931594400426694
         }
       },
-      "curvature" : 0.06662595941346228
+      "curvature" : 0.132148448806923
     }, {
-      "time" : 1.0622286808047574,
-      "velocity" : 3.0,
-      "acceleration" : -0.31635458303853925,
+      "time" : 1.1161884359145708,
+      "velocity" : 2.962090431939117,
+      "acceleration" : -0.3477272761374577,
       "pose" : {
         "translation" : {
-          "x" : 3.2217983152980736,
-          "y" : -2.0653284248280315
+          "x" : 3.0209465773668853,
+          "y" : -2.0757587164407596
         },
         "rotation" : {
-          "radians" : 3.0022270936000854
+          "radians" : 3.1052018923916562
         }
       },
-      "curvature" : 0.08413564313074447
+      "curvature" : 0.14325696560995094
     }, {
-      "time" : 1.0860719059198862,
-      "velocity" : 2.9924570864604094,
-      "acceleration" : -0.8283229299033826,
+      "time" : 1.1455151922201363,
+      "velocity" : 2.9518927188510355,
+      "acceleration" : -0.390660961922364,
       "pose" : {
         "translation" : {
-          "x" : 3.1510195962328633,
-          "y" : -2.0556324955958685
+          "x" : 2.9342666338240138,
+          "y" : -2.0731572984038564
         },
         "rotation" : {
-          "radians" : 3.0088968912951697
+          "radians" : 3.118160641588388
         }
       },
-      "curvature" : 0.10283063070655941
+      "curvature" : 0.15590602336612916
     }, {
-      "time" : 1.1097884594192817,
-      "velocity" : 2.97281212137858,
-      "acceleration" : -0.8752690242844084,
+      "time" : 1.174600881740302,
+      "velocity" : 2.9405300754049124,
+      "acceleration" : -0.49735381964647707,
       "pose" : {
         "translation" : {
-          "x" : 3.0808679684996605,
-          "y" : -2.04654473811388
+          "x" : 2.848585784150231,
+          "y" : -2.071738858124263
         },
         "rotation" : {
-          "radians" : 3.016870931220585
+          "radians" : 3.1321236991006915
         }
       },
-      "curvature" : 0.12290478806154469
+      "curvature" : 0.17032641010647295
     }, {
-      "time" : 1.1333958560398858,
-      "velocity" : 2.952149298372569,
-      "acceleration" : -1.014196696356634,
+      "time" : 1.203394839648812,
+      "velocity" : 2.926209290456375,
+      "acceleration" : -0.6832682280406255,
       "pose" : {
         "translation" : {
-          "x" : 3.0114359337338006,
-          "y" : -2.0381601940104916
+          "x" : 2.7641226388952367,
+          "y" : -2.071565490563131
         },
         "rotation" : {
-          "radians" : 3.0262126639182343
+          "radians" : -3.135997290868943
         }
       },
-      "curvature" : 0.1445715103185185
+      "curvature" : 0.18678355815727837
     }, {
-      "time" : 1.156876767647848,
-      "velocity" : 2.9283350353923314,
-      "acceleration" : -3.999999999999987,
+      "time" : 1.2318683790866636,
+      "velocity" : 2.9067542256186294,
+      "acceleration" : -1.2358898220357044,
       "pose" : {
         "translation" : {
-          "x" : 2.942814587886815,
-          "y" : -2.030572531226916
+          "x" : 2.6810877616808284,
+          "y" : -2.07269494375214
         },
         "rotation" : {
-          "radians" : 3.0369917546143315
+          "radians" : -3.1197253477859697
         }
       },
-      "curvature" : 0.16806335127014238
+      "curvature" : 0.20558054021946048
     }, {
-      "time" : 1.1804969687914266,
-      "velocity" : 2.8338542308180172,
-      "acceleration" : -4.000000000000006,
+      "time" : 1.2600559690912383,
+      "velocity" : 2.8719174700242602,
+      "acceleration" : -4.000000000000005,
       "pose" : {
         "translation" : {
-          "x" : 2.875093082553805,
-          "y" : -2.0238734903460447
+          "x" : 2.5996822740554615,
+          "y" : -2.0751801623419794
         },
         "rotation" : {
-          "radians" : 3.049284132432743
+          "radians" : -3.1021297315977163
         }
       },
-      "curvature" : 0.19363060644268892
+      "curvature" : 0.2270598313832427
     }, {
-      "time" : 1.2045405378384517,
-      "velocity" : 2.737679954629917,
+      "time" : 1.288358606349635,
+      "velocity" : 2.758706920990673,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.8083580863008137,
-          "y" : -2.018152330921339
+          "x" : 2.5200964603488103,
+          "y" : -2.0790688311508347
         },
         "rotation" : {
-          "radians" : 3.0631717946866663
+          "radians" : -3.0830847917195086
         }
       },
-      "curvature" : 0.22153825590519519
+      "curvature" : 0.25160273261753874
     }, {
-      "time" : 1.2290242925168728,
-      "velocity" : 2.639744935916233,
+      "time" : 1.3171507941983114,
+      "velocity" : 2.6435381695959674,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.742693245992202,
-          "y" : -2.0134952778057222
+          "x" : 2.442508372526329,
+          "y" : -2.0844029187128683
         },
         "rotation" : {
-          "radians" : 3.078742282815308
+          "radians" : -3.0624558861984505
         }
       },
-      "curvature" : 0.25206048276332627
+      "curvature" : 0.27962477885484877
     }, {
-      "time" : 1.2539716924931896,
-      "velocity" : 2.5399553360109657,
-      "acceleration" : -4.0,
+      "time" : 1.3464486290742,
+      "velocity" : 2.526346830092413,
+      "acceleration" : -4.000000000000006,
       "pose" : {
         "translation" : {
-          "x" : 2.6781786481180205,
-          "y" : -2.00998496748047
+          "x" : 2.3670824350438124,
+          "y" : -2.091218220826704
         },
         "rotation" : {
-          "radians" : 3.096087722144318
+          "radians" : -3.0401007894747756
         }
       },
-      "curvature" : 0.28547175988586754
+      "curvature" : 0.3115646754019423
     }, {
-      "time" : 1.3053939989160892,
-      "velocity" : 2.3342661103193674,
-      "acceleration" : -3.9999999999999964,
+      "time" : 1.376280964585666,
+      "velocity" : 2.4070174880465487,
+      "acceleration" : -4.000000000000006,
       "pose" : {
         "translation" : {
-          "x" : 2.5528994917258387,
-          "y" : -2.006713857241266
+          "x" : 2.293968049701957,
+          "y" : -2.0995439041039106
         },
         "rotation" : {
-          "radians" : 3.136484953437787
+          "radians" : -3.015872245931216
         }
       },
-      "curvature" : 0.3619731146711039
+      "curvature" : 0.34786330489418216
     }, {
-      "time" : 1.3592200941345929,
-      "velocity" : 2.1189617294453527,
-      "acceleration" : -3.689061390388735,
+      "time" : 1.4066937018304468,
+      "velocity" : 2.285366539067425,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.433069631998663,
-          "y" : -2.0089072851188305
+          "x" : 2.2232982005009205,
+          "y" : -2.1094020495174846
         },
         "rotation" : {
-          "radians" : -3.098071189329059
+          "radians" : -2.9896221228967343
         }
       },
-      "curvature" : 0.45256136707383543
+      "curvature" : 0.38892817020202786
     }, {
-      "time" : 1.4159189374172112,
-      "velocity" : 1.9097962158117439,
-      "acceleration" : -2.8753503248023993,
+      "time" : 1.4377557412227517,
+      "velocity" : 2.161118381498206,
+      "acceleration" : -3.6964624250106115,
       "pose" : {
         "translation" : {
-          "x" : 2.3191466427738305,
-          "y" : -2.0170406871309297
+          "x" : 2.1551880584948853,
+          "y" : -2.1208071959503343
         },
         "rotation" : {
-          "radians" : -3.040577368313093
+          "radians" : -2.961207726869443
         }
       },
-      "curvature" : 0.5571211509945629
+      "curvature" : 0.4350774646434125
     }, {
-      "time" : 1.4754694675169775,
-      "velocity" : 1.7385675797472258,
-      "acceleration" : -2.1691436818120216,
+      "time" : 1.469492309203223,
+      "velocity" : 2.043805350459599,
+      "acceleration" : -3.2130617613339636,
       "pose" : {
         "translation" : {
-          "x" : 2.211479419327574,
-          "y" : -2.0314789329225036
+          "x" : 2.0897335866466165,
+          "y" : -2.133765883743763
         },
         "rotation" : {
-          "radians" : -2.9739098912623314
+          "radians" : -2.9305009432432754
         }
       },
-      "curvature" : 0.6722650755346324
+      "curvature" : 0.4864572049110892
     }, {
-      "time" : 1.5372939266887176,
-      "velocity" : 1.6044614447534007,
-      "acceleration" : -1.5165826850953532,
+      "time" : 1.5346845087245022,
+      "velocity" : 1.8343387870405228,
+      "acceleration" : -2.5376336019641084,
       "pose" : {
         "translation" : {
-          "x" : 2.110290940850973,
-          "y" : -2.0524586082901806
+          "x" : 1.9670710939447247,
+          "y" : -2.1643273133604453
         },
         "rotation" : {
-          "radians" : -2.898381476531495
+          "radians" : -2.8618508313251305
         }
       },
-      "curvature" : 0.7893418563615717
+      "curvature" : 0.603899409578728
     }, {
-      "time" : 1.6006283768070562,
-      "velocity" : 1.508409514333893,
-      "acceleration" : -1.032513464911367,
+      "time" : 1.6017399968098842,
+      "velocity" : 1.6641765272789533,
+      "acceleration" : -1.8083036583159366,
       "pose" : {
         "translation" : {
-          "x" : 2.0156610329259053,
-          "y" : -2.08007029770679
+          "x" : 1.8556412487423586,
+          "y" : -2.200961345108226
         },
         "rotation" : {
-          "radians" : -2.8152839089339228
+          "radians" : -2.783526515109936
         }
       },
-      "curvature" : 0.8930693351434651
+      "curvature" : 0.7337108127806126
     }, {
-      "time" : 1.6326245076366628,
-      "velocity" : 1.4753730784272583,
-      "acceleration" : -0.7012265516486391,
+      "time" : 1.6696662033549567,
+      "velocity" : 1.5413453194879747,
+      "acceleration" : -1.1909025235769102,
       "pose" : {
         "translation" : {
-          "x" : 1.9707871028025834,
-          "y" : -2.0963471729342444
+          "x" : 1.7553779586158669,
+          "y" : -2.2433857961650574
         },
         "rotation" : {
-          "radians" : -2.77164828786782
+          "radians" : -2.696866534237099
         }
       },
-      "curvature" : 0.9335121925643923
+      "curvature" : 0.8553105081087353
     }, {
-      "time" : 1.6646096086435074,
-      "velocity" : 1.4529442763440952,
-      "acceleration" : -0.35056590766525175,
+      "time" : 1.7035990274978792,
+      "velocity" : 1.500934633584077,
+      "acceleration" : -0.7132384921286533,
       "pose" : {
         "translation" : {
-          "x" : 1.927509130000999,
-          "y" : -2.1142408668458756
+          "x" : 1.7092936806082193,
+          "y" : -2.266634670725722
         },
         "rotation" : {
-          "radians" : -2.727199104287848
+          "radians" : -2.651456117134893
         }
       },
-      "curvature" : 0.9625555150908901
+      "curvature" : 0.9019867248099454
     }, {
-      "time" : 1.696419570306363,
-      "velocity" : 1.4417927882609594,
-      "acceleration" : 0.03358024482766757,
+      "time" : 1.7371443955145358,
+      "velocity" : 1.477008785881976,
+      "acceleration" : -0.14025816644194736,
       "pose" : {
         "translation" : {
-          "x" : 1.8857891546813712,
-          "y" : -2.1337137498151666
+          "x" : 1.66577386771678,
+          "y" : -2.291146687697619
         },
         "rotation" : {
-          "radians" : -2.6824765979300107
+          "radians" : -2.6055827914929948
         }
       },
-      "curvature" : 0.9775027904601952
+      "curvature" : 0.931445708792421
     }, {
-      "time" : 1.7278732271361714,
-      "velocity" : 1.4428490097580298,
-      "acceleration" : 0.7353110075222659,
+      "time" : 1.7700077731011385,
+      "velocity" : 1.4723994287985898,
+      "acceleration" : -0.36599934740864887,
       "pose" : {
         "translation" : {
-          "x" : 1.845577037867585,
-          "y" : -2.154715745106209
+          "x" : 1.6246788285368083,
+          "y" : -2.316835860229361
         },
         "rotation" : {
-          "radians" : -2.638094148131616
+          "radians" : -2.5601885277763987
         }
       },
-      "curvature" : 0.9760721742923996
+      "curvature" : 0.9372866321152672
     }, {
-      "time" : 1.7887154459118286,
-      "velocity" : 1.4875869629458485,
-      "acceleration" : -0.7264993431499159,
+      "time" : 1.8021746246681412,
+      "velocity" : 1.460626382116876,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 1.7694116961356485,
-          "y" : -2.201041208136303
+          "x" : 1.5858357121177562,
+          "y" : -2.3436036384127874
         },
         "rotation" : {
-          "radians" : -2.5530618886531005
+          "radians" : -2.5164028420542666
         }
       },
-      "curvature" : 0.9182458546356818
+      "curvature" : 0.9134408086162918
     }, {
-      "time" : 1.8485897368265631,
-      "velocity" : 1.444088329924727,
-      "acceleration" : -4.000000000000003,
+      "time" : 1.871161022380072,
+      "velocity" : 1.1846807912691524,
+      "acceleration" : -3.9999999999999996,
       "pose" : {
         "translation" : {
-          "x" : 1.6983479407097803,
-          "y" : -2.2525466603589237
+          "x" : 1.5140396751585286,
+          "y" : -2.399915258109104
         },
         "rotation" : {
-          "radians" : -2.477748499042142
+          "radians" : -2.438909851704161
         }
       },
-      "curvature" : 0.7850323243866926
+      "curvature" : 0.7583879709418248
     }, {
-      "time" : 1.9149904498064472,
-      "velocity" : 1.178485478005191,
+      "time" : 1.9587433944421073,
+      "velocity" : 0.8343513030210117,
       "acceleration" : -4.000000000000001,
       "pose" : {
         "translation" : {
-          "x" : 1.6314912662651295,
-          "y" : -2.308326917495606
-        },
-        "rotation" : {
-          "radians" : -2.417734046615386
-        }
-      },
-      "curvature" : 0.5823128416173662
-    }, {
-      "time" : 2.0013081447960555,
-      "velocity" : 0.8332146980467585,
-      "acceleration" : -3.9999999999999996,
-      "pose" : {
-        "translation" : {
-          "x" : 1.5677005887233548,
-          "y" : -2.3672244890911642
+          "x" : 1.4482867427918507,
+          "y" : -2.4590245412282457
         },
         "rotation" : {
-          "radians" : -2.378189328750247
+          "radians" : -2.384362556563286
         }
       },
-      "curvature" : 0.3190107323302002
+      "curvature" : 0.4486116146017388
     }, {
-      "time" : 2.209611819307745,
+      "time" : 2.1673312201973602,
       "velocity" : 0.0,
-      "acceleration" : -3.9999999999999996,
+      "acceleration" : -4.000000000000001,
       "pose" : {
         "translation" : {
-          "x" : 1.5055710077285767,
-          "y" : -2.427811861038208
+          "x" : 1.3858580589294434,
+          "y" : -2.519644260406494
         },
         "rotation" : {
           "radians" : -2.3639463990252185
@@ -1473,11 +1459,11 @@
       "curvature" : 0.0
     } ],
     "pointList" : [ {
-      "x" : [ 5.270317554473877, -3.944063901901245, 0.0 ],
-      "y" : [ -2.2654507160186768, -0.02777838706970215, 0.0 ]
+      "x" : [ 5.218771457672119, -1.5258679389953613, 0.0 ],
+      "y" : [ -2.376249313354492, 0.3001227378845215, 0.0 ]
     }, {
-      "x" : [ 1.5055710077285767, -1.9787755012512207, 0.0 ],
-      "y" : [ -2.427811861038208, -1.9483323097229004, 0.0 ]
+      "x" : [ 1.3858580589294434, -1.9787755012512207, 0.0 ],
+      "y" : [ -2.519644260406494, -1.9483323097229004, 0.0 ]
     } ],
     "rotations" : [ {
       "time" : 0.0,
@@ -1485,7 +1471,7 @@
         "radians" : -2.3736477827122884
       }
     }, {
-      "time" : 2.1199999526143074,
+      "time" : 2.079999953508377,
       "rotation" : {
         "radians" : -2.3824463294692526
       }
@@ -1541,11 +1527,11 @@
     "states" : [ {
       "time" : 0.0,
       "velocity" : 0.0,
-      "acceleration" : 4.000000000000001,
+      "acceleration" : 3.999999999999999,
       "pose" : {
         "translation" : {
-          "x" : 1.5055710077285767,
-          "y" : -2.427811861038208
+          "x" : 1.3858580589294434,
+          "y" : -2.519644260406494
         },
         "rotation" : {
           "radians" : 0.0
@@ -1553,223 +1539,349 @@
       },
       "curvature" : 0.0
     }, {
-      "time" : 0.24913945161581508,
-      "velocity" : 0.9965578064632605,
-      "acceleration" : 3.999999999999997,
+      "time" : 0.2506613508834166,
+      "velocity" : 1.0026454035336663,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 1.6297117822080054,
-          "y" : -2.4280100629130175
+          "x" : 1.5115202584765939,
+          "y" : -2.5197252612570082
         },
         "rotation" : {
-          "radians" : -0.004695040376802698
+          "radians" : -0.0018522539500240682
         }
       },
-      "curvature" : -0.07299386363005499
+      "curvature" : -0.026842794063227322
     }, {
-      "time" : 0.3491337812502485,
-      "velocity" : 1.396535125000994,
-      "acceleration" : 3.999999999999997,
+      "time" : 0.30789720119155506,
+      "velocity" : 1.23158880476622,
+      "acceleration" : 4.000000000000002,
       "pose" : {
         "translation" : {
-          "x" : 1.7493532588196103,
-          "y" : -2.4292461404693313
+          "x" : 1.5754591548339363,
+          "y" : -2.519904416002504
         },
         "rotation" : {
-          "radians" : -0.017229353546914548
+          "radians" : -0.003832966982418239
         }
       },
-      "curvature" : -0.136406473250297
+      "curvature" : -0.03448311037462077
     }, {
-      "time" : 0.42199180876246867,
-      "velocity" : 1.6879672350498744,
-      "acceleration" : 4.000000000000002,
+      "time" : 0.35684276742474663,
+      "velocity" : 1.4273710696989863,
+      "acceleration" : 3.9999999999999964,
       "pose" : {
         "translation" : {
-          "x" : 1.8616806058923885,
-          "y" : -2.4321694050941005
+          "x" : 1.6405304865329526,
+          "y" : -2.520230419613654
         },
         "rotation" : {
-          "radians" : -0.03593834139063176
+          "radians" : -0.006233177930525208
         }
       },
-      "curvature" : -0.19668533798708224
+      "curvature" : -0.03877108765930074
     }, {
-      "time" : 0.4796162909188312,
-      "velocity" : 1.9184651636753245,
-      "acceleration" : 3.9999999999999956,
+      "time" : 0.40065288496208495,
+      "velocity" : 1.6026115398483394,
+      "acceleration" : 3.9999999999999933,
       "pose" : {
         "translation" : {
-          "x" : 1.9654748018365353,
-          "y" : -2.4370610546320677
+          "x" : 1.7069005447805807,
+          "y" : -2.5207311531334256
         },
         "rotation" : {
-          "radians" : -0.059144691931219094
+          "radians" : -0.008873618264835237
         }
       },
-      "curvature" : -0.2480691940003244
+      "curvature" : -0.04040754941441055
     }, {
-      "time" : 0.5271053063257927,
-      "velocity" : 2.10842122530317,
-      "acceleration" : 3.999999999999995,
+      "time" : 0.44090317601445284,
+      "velocity" : 1.7636127040578107,
+      "acceleration" : 4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.0608466667965786,
-          "y" : -2.4438955256428017
+          "x" : 1.7746427433321514,
+          "y" : -2.5214250950984933
         },
         "rotation" : {
-          "radians" : -0.08433384568667432
+          "radians" : -0.011609841654107651
         }
       },
-      "curvature" : -0.27258387491895875
+      "curvature" : -0.04010473649023733
     }, {
-      "time" : 0.5675446352717414,
-      "velocity" : 2.270178541086965,
-      "acceleration" : 3.9999999999999947,
+      "time" : 0.4784875189105668,
+      "velocity" : 1.9139500756422665,
+      "acceleration" : 3.9999999999999907,
       "pose" : {
         "translation" : {
-          "x" : 2.1489708943045116,
-          "y" : -2.4524018456577323
+          "x" : 1.843746311953339,
+          "y" : -2.522322105081429
         },
         "rotation" : {
-          "radians" : -0.10771687746023423
+          "radians" : -0.014331201202338068
         }
       },
-      "curvature" : -0.24453210747816045
+      "curvature" : -0.03847972749251971
     }, {
-      "time" : 0.6031714577725938,
-      "velocity" : 2.412685831090375,
-      "acceleration" : 4.000000000000005,
+      "time" : 0.5139493740375787,
+      "velocity" : 2.055797496150314,
+      "acceleration" : 3.999999999999994,
       "pose" : {
         "translation" : {
-          "x" : 2.231820082932927,
-          "y" : -2.4621249854371854
+          "x" : 1.9141249898821115,
+          "y" : -2.5234242072328925
         },
         "rotation" : {
-          "radians" : -0.12458825964417336
+          "radians" : -0.016956454931120607
         }
       },
-      "curvature" : -0.1479406947072193
+      "curvature" : -0.03601472391776367
     }, {
-      "time" : 0.6357585844969933,
-      "velocity" : 2.5430343379879727,
-      "acceleration" : -3.999999999999978,
+      "time" : 0.5476314929124355,
+      "velocity" : 2.190525971649741,
+      "acceleration" : 4.000000000000006,
       "pose" : {
         "translation" : {
-          "x" : 2.3118987679481506,
-          "y" : -2.472487211227417
+          "x" : 1.985625719290681,
+          "y" : -2.5247263738238246
         },
         "rotation" : {
-          "radians" : -0.1307784900417709
+          "radians" : -0.019428093132643293
         }
       },
-      "curvature" : 0.0
+      "curvature" : -0.03305737128049612
     }, {
-      "time" : 0.6683457112213927,
-      "velocity" : 2.412685831090376,
-      "acceleration" : -4.000000000000005,
+      "time" : 0.5797531676739855,
+      "velocity" : 2.3190126706959413,
+      "acceleration" : 4.000000000000006,
       "pose" : {
         "translation" : {
-          "x" : 2.391977452963374,
-          "y" : -2.4828494370176486
+          "x" : 2.0580373387474538,
+          "y" : -2.5262173087876363
         },
         "rotation" : {
-          "radians" : -0.12458825964417336
+          "radians" : -0.021706661066398916
         }
       },
-      "curvature" : 0.1479406947072193
+      "curvature" : -0.02983965013490699
     }, {
-      "time" : 0.7039725337222453,
-      "velocity" : 2.2701785410869655,
-      "acceleration" : -4.000000000000005,
+      "time" : 0.6104540649254343,
+      "velocity" : 2.4418162597017363,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.131099276678981,
+          "y" : -2.527880231262401
+        },
+        "rotation" : {
+          "radians" : -0.023765711637107785
+        }
+      },
+      "curvature" : -0.026501856917817767
+    }, {
+      "time" : 0.6398209489868676,
+      "velocity" : 2.5592837959474695,
+      "acceleration" : 3.999999999999994,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2045102448319085,
+          "y" : -2.529693659133045
+        },
+        "rotation" : {
+          "radians" : -0.025587608770040265
+        }
+      },
+      "curvature" : -0.02311472481731095
+    }, {
+      "time" : 0.6679049204495552,
+      "velocity" : 2.67161968179822,
+      "acceleration" : 2.6312398843848555,
+      "pose" : {
+        "translation" : {
+          "x" : 2.277936931734928,
+          "y" : -2.5316321925735394
+        },
+        "rotation" : {
+          "radians" : -0.027160172194753095
+        }
+      },
+      "curvature" : -0.019696900519105796
+    }, {
+      "time" : 0.6949126774005732,
+      "velocity" : 2.7426835690755107,
+      "acceleration" : -3.999999999999994,
       "pose" : {
         "translation" : {
-          "x" : 2.4748266415917897,
-          "y" : -2.4925725767971016
+          "x" : 2.351022696160726,
+          "y" : -2.5336672975890906
         },
         "rotation" : {
-          "radians" : -0.10771687746023423
+          "radians" : -0.02847404834663094
         }
       },
-      "curvature" : 0.24453210747816045
+      "curvature" : -0.01622720956565302
     }, {
-      "time" : 0.7444118626681941,
-      "velocity" : 2.10842122530317,
-      "acceleration" : -3.999999999999998,
+      "time" : 0.7218404114156483,
+      "velocity" : 2.6349726330152103,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4233962605879356,
+          "y" : -2.5357680895583314
+        },
+        "rotation" : {
+          "radians" : -0.02952065953655597
+        }
+      },
+      "curvature" : -0.012652107561005304
+    }, {
+      "time" : 0.7494857099529498,
+      "velocity" : 2.524391438866004,
+      "acceleration" : -3.999999999999994,
       "pose" : {
         "translation" : {
-          "x" : 2.5629508690997227,
-          "y" : -2.5010788968120323
+          "x" : 2.494680404663086,
+          "y" : -2.5379021167755127
         },
         "rotation" : {
-          "radians" : -0.08433384568667432
+          "radians" : -0.030290590291302242
         }
       },
-      "curvature" : 0.27258387491895875
+      "curvature" : -0.008889004344138316
     }, {
-      "time" : 0.7919008780751556,
-      "velocity" : 1.9184651636753245,
+      "time" : 0.7777916661649981,
+      "velocity" : 2.411167614017811,
       "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.658322734059766,
-          "y" : -2.5079133678227663
+          "x" : 2.5645006586625527,
+          "y" : -2.540036143992694
         },
         "rotation" : {
-          "radians" : -0.059144691931219094
+          "radians" : -0.030772296527561225
         }
       },
-      "curvature" : 0.2480691940003244
+      "curvature" : -0.004826154357885299
     }, {
-      "time" : 0.8495253602315181,
-      "velocity" : 1.6879672350498744,
-      "acceleration" : -3.9999999999999982,
+      "time" : 0.8066975299375645,
+      "velocity" : 2.2955441589275454,
+      "acceleration" : -4.000000000000007,
       "pose" : {
         "translation" : {
-          "x" : 2.762116930003913,
-          "y" : -2.5128050173607335
+          "x" : 2.6324939969545085,
+          "y" : -2.542136935961935
         },
         "rotation" : {
-          "radians" : -0.03593834139063176
+          "radians" : -0.030951061958126948
         }
       },
-      "curvature" : 0.19668533798708224
+      "curvature" : -3.197882350940787E-4
     }, {
-      "time" : 0.9223833877437382,
-      "velocity" : 1.3965351250009943,
-      "acceleration" : -3.9999999999999996,
+      "time" : 0.8361410171738428,
+      "velocity" : 2.177770209982432,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.874444277076691,
-          "y" : -2.5157282819855027
+          "x" : 2.698317531460873,
+          "y" : -2.544172040977486
         },
         "rotation" : {
-          "radians" : -0.017229353546914548
+          "radians" : -0.030808177370938297
         }
       },
-      "curvature" : 0.136406473250297
+      "curvature" : 0.004810664209664993
     }, {
-      "time" : 1.0223777173781716,
-      "velocity" : 0.9965578064632606,
-      "acceleration" : -4.000000000000001,
+      "time" : 0.8660614348369912,
+      "velocity" : 2.0580885393298387,
+      "acceleration" : -4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7616572051192634,
+          "y" : -2.5461105744179804
+        },
+        "rotation" : {
+          "radians" : -0.030320390272959595
+        }
+      },
+      "curvature" : 0.010787761573423147
+    }, {
+      "time" : 0.9271262518256638,
+      "velocity" : 1.8138292713751483,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.8798250574927806,
+          "y" : -2.549586924763389
+        },
+        "rotation" : {
+          "radians" : -0.02819439651168064
+        }
+      },
+      "curvature" : 0.02638800187200745
+    }, {
+      "time" : 0.989659585589217,
+      "velocity" : 1.5636959363209353,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9853920694440603,
+          "y" : -2.552380026318133
+        },
+        "rotation" : {
+          "radians" : -0.02431565149909576
+        }
+      },
+      "curvature" : 0.04885357015887642
+    }, {
+      "time" : 1.054053992446091,
+      "velocity" : 1.3061183088934392,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 3.077770434158083,
+          "y" : -2.554379138452532
+        },
+        "rotation" : {
+          "radians" : -0.018496813941660972
+        }
+      },
+      "curvature" : 0.07878050827674199
+    }, {
+      "time" : 1.1223816921391718,
+      "velocity" : 1.0328075101211158,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.1576682134182192,
+          "y" : -2.5555738139373716
+        },
+        "rotation" : {
+          "radians" : -0.01102726873603195
+        }
+      },
+      "curvature" : 0.10654928830990536
+    }, {
+      "time" : 1.2022085804462013,
+      "velocity" : 0.7134999568929983,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 2.994085753688296,
-          "y" : -2.5169643595418165
+          "x" : 3.2273675283886405,
+          "y" : -2.556078972294017
         },
         "rotation" : {
-          "radians" : -0.004695040376802698
+          "radians" : -0.003571825888394278
         }
       },
-      "curvature" : 0.07299386363005499
+      "curvature" : 0.09616404801082394
     }, {
-      "time" : 1.2715171689939868,
+      "time" : 1.3805835696694508,
       "velocity" : 0.0,
-      "acceleration" : -4.000000000000001,
+      "acceleration" : -4.0,
       "pose" : {
         "translation" : {
-          "x" : 3.1182265281677246,
-          "y" : -2.517162561416626
+          "x" : 3.2910027503967285,
+          "y" : -2.5561599731445312
         },
         "rotation" : {
           "radians" : 0.0
@@ -1778,11 +1890,11 @@
       "curvature" : 0.0
     } ],
     "pointList" : [ {
-      "x" : [ 1.5055710077285767, 2.0, 0.0 ],
-      "y" : [ -2.427811861038208, 0.0, 0.0 ]
+      "x" : [ 1.3858580589294434, 2.0, 0.0 ],
+      "y" : [ -2.519644260406494, 0.0, 0.0 ]
     }, {
-      "x" : [ 3.1182265281677246, 2.0, 0.0 ],
-      "y" : [ -2.517162561416626, 0.0, 0.0 ]
+      "x" : [ 3.2910027503967285, 1.0, 0.0 ],
+      "y" : [ -2.5561599731445312, 0.0, 0.0 ]
     } ],
     "rotations" : [ {
       "time" : 0.0,
@@ -1790,7 +1902,7 @@
         "radians" : -2.670353755551324
       }
     }, {
-      "time" : 1.1799999736249447,
+      "time" : 1.289999971166253,
       "rotation" : {
         "radians" : -2.684023492023121
       }
@@ -1803,7 +1915,7 @@
     "constraints" : [ ]
   }, {
     "type" : "script",
-    "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 20\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF",
+    "script" : "#Intake.setWantedIntakeState OFF\nVisionManager.shootBalls 8.5\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF\nIntake.setIntakeSolState CLOSE",
     "closed" : false,
     "valid" : true,
     "sendableScript" : {
@@ -1811,7 +1923,7 @@
       "delay" : 0.0,
       "commands" : [ {
         "methodName" : "frc.subsystem.VisionManager.shootBalls",
-        "args" : [ "20" ],
+        "args" : [ "8.5" ],
         "argTypes" : [ "double" ],
         "reflection" : true
       }, {
@@ -1824,7 +1936,767 @@
         "args" : [ "OFF" ],
         "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
         "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "CLOSE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
       } ]
     }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2910027503967285,
+          "y" : -2.5561599731445312
+        },
+        "rotation" : {
+          "radians" : -0.2884106402522736
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.23642508906658902,
+      "velocity" : 0.9457003562663563,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3981667469918193,
+          "y" : -2.5879985818380646
+        },
+        "rotation" : {
+          "radians" : -0.2891712372126716
+        }
+      },
+      "curvature" : -5.874320584109886E-4
+    }, {
+      "time" : 0.3444359926496966,
+      "velocity" : 1.3777439705987866,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5184656394194462,
+          "y" : -2.6236786987501546
+        },
+        "rotation" : {
+          "radians" : -0.286576361816551
+        }
+      },
+      "curvature" : 0.04203266118630113
+    }, {
+      "time" : 0.3927805454968463,
+      "velocity" : 1.5711221819873853,
+      "acceleration" : 3.9999999999999942,
+      "pose" : {
+        "translation" : {
+          "x" : 3.586874350548449,
+          "y" : -2.643708721189732
+        },
+        "rotation" : {
+          "radians" : -0.2828782585831171
+        }
+      },
+      "curvature" : 0.06060524122763304
+    }, {
+      "time" : 0.43985280368645924,
+      "velocity" : 1.7594112147458367,
+      "acceleration" : 3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 3.662202759596539,
+          "y" : -2.665394846008212
+        },
+        "rotation" : {
+          "radians" : -0.277565073334469
+        }
+      },
+      "curvature" : 0.07384725863707324
+    }, {
+      "time" : 0.4864477060306216,
+      "velocity" : 1.9457908241224862,
+      "acceleration" : 3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.745298805531398,
+          "y" : -2.688772057520943
+        },
+        "rotation" : {
+          "radians" : -0.2707950380725529
+        }
+      },
+      "curvature" : 0.08213625833510316
+    }, {
+      "time" : 0.5329979230669721,
+      "velocity" : 2.131991692267888,
+      "acceleration" : 4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8368513446766883,
+          "y" : -2.7137960128020495
+        },
+        "rotation" : {
+          "radians" : -0.2627587722278301
+        }
+      },
+      "curvature" : 0.08660282922194644
+    }, {
+      "time" : 0.5797279435183833,
+      "velocity" : 2.3189117740735328,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9373990498514004,
+          "y" : -2.740351675968121
+        },
+        "rotation" : {
+          "radians" : -0.25363830846069757
+        }
+      },
+      "curvature" : 0.08841580171796991
+    }, {
+      "time" : 0.6267362610549605,
+      "velocity" : 2.5069450442198415,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.047339309509198,
+          "y" : -2.7682619524618985
+        },
+        "rotation" : {
+          "radians" : -0.2435910468434367
+        }
+      },
+      "curvature" : 0.0885183696531404
+    }, {
+      "time" : 0.6740431507892504,
+      "velocity" : 2.696172603157001,
+      "acceleration" : 3.9999999999999862,
+      "pose" : {
+        "translation" : {
+          "x" : 4.166937126877762,
+          "y" : -2.7972963233359636
+        },
+        "rotation" : {
+          "radians" : -0.2327470040418315
+        }
+      },
+      "curvature" : 0.08758788738466654
+    }, {
+      "time" : 0.6978010763245834,
+      "velocity" : 2.791204305298333,
+      "acceleration" : 3.999999999999974,
+      "pose" : {
+        "translation" : {
+          "x" : 4.230406424149577,
+          "y" : -2.8121505236061024
+        },
+        "rotation" : {
+          "radians" : -0.22706006402416112
+        }
+      },
+      "curvature" : 0.08688404973771191
+    }, {
+      "time" : 0.7216202590268884,
+      "velocity" : 2.8864810361075524,
+      "acceleration" : 3.9999999999999747,
+      "pose" : {
+        "translation" : {
+          "x" : 4.29633401909814,
+          "y" : -2.827179479536426
+        },
+        "rotation" : {
+          "radians" : -0.22121199588447016
+        }
+      },
+      "curvature" : 0.08607766603782731
+    }, {
+      "time" : 0.74549257464285,
+      "velocity" : 2.9819702985713983,
+      "acceleration" : 0.7442098400976622,
+      "pose" : {
+        "translation" : {
+          "x" : 4.364719766680094,
+          "y" : -2.8423431956032283
+        },
+        "rotation" : {
+          "radians" : -0.21521310387858186
+        }
+      },
+      "curvature" : 0.08519994915603576
+    }, {
+      "time" : 0.7697192113482115,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.1864982661111718E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.435556916364085,
+          "y" : -2.8575999561866112
+        },
+        "rotation" : {
+          "radians" : -0.2090728501840574
+        }
+      },
+      "curvature" : 0.08427499675849662
+    }, {
+      "time" : 0.7946715843679995,
+      "velocity" : 3.0,
+      "acceleration" : -1.1500748104073835E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.508832390228868,
+          "y" : -2.8729065953918482
+        },
+        "rotation" : {
+          "radians" : -0.2028000166117804
+        }
+      },
+      "curvature" : 0.08332106007566421
+    }, {
+      "time" : 0.8204142117652993,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.1162447712385717E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.584527061061408,
+          "y" : -2.88821876687075
+        },
+        "rotation" : {
+          "radians" : -0.19640285659122264
+        }
+      },
+      "curvature" : 0.08235160626948937
+    }, {
+      "time" : 0.8469370211760767,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.6626160304549895,
+          "y" : -2.9034912136430293
+        },
+        "rotation" : {
+          "radians" : -0.18988923719000408
+        }
+      },
+      "curvature" : 0.0813761949653504
+    }, {
+      "time" : 0.874228259006809,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.743068906907318,
+          "y" : -2.9186780379176653
+        },
+        "rotation" : {
+          "radians" : -0.18326677164075206
+        }
+      },
+      "curvature" : 0.08040119291556454
+    }, {
+      "time" : 0.9022745981913911,
+      "velocity" : 3.0,
+      "acceleration" : -1.0284611572537565E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.825850083918625,
+          "y" : -2.9337329709142717
+        },
+        "rotation" : {
+          "radians" : -0.17654294326329226
+        }
+      },
+      "curvature" : 0.07943035076297603
+    }, {
+      "time" : 0.9310612442543232,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.0032243113209958E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.910919018089771,
+          "y" : -2.9486096426844597
+        },
+        "rotation" : {
+          "radians" : -0.16972522188764874
+        }
+      },
+      "curvature" : 0.07846526410378302
+    }, {
+      "time" : 0.9605720395861806,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.998230507220355,
+          "y" : -2.9632618519332032
+        },
+        "rotation" : {
+          "radians" : -0.16282117397282284
+        }
+      },
+      "curvature" : 0.07750573849274361
+    }, {
+      "time" : 0.990789565834994,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.087734968406814,
+          "y" : -2.9776438358402046
+        },
+        "rotation" : {
+          "radians" : -0.15583856762569545
+        }
+      },
+      "curvature" : 0.07655007526138907
+    }, {
+      "time" : 1.0216952443158518,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.179378716140528,
+          "y" : -2.991710539881261
+        },
+        "rotation" : {
+          "radians" : -0.14878547368600106
+        }
+      },
+      "curvature" : 0.07559529234150263
+    }, {
+      "time" : 1.0532694343409463,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.273104240405928,
+          "y" : -3.005417887649628
+        },
+        "rotation" : {
+          "radians" : -0.14167036397863442
+        }
+      },
+      "curvature" : 0.07463729185441531
+    }, {
+      "time" : 1.0854915293736447,
+      "velocity" : 3.0,
+      "acceleration" : -2.7038611726562314E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 5.368850484778598,
+          "y" : -3.0187230506773854
+        },
+        "rotation" : {
+          "radians" : -0.13450220775752084
+        }
+      },
+      "curvature" : 0.07367098410111733
+    }, {
+      "time" : 1.1183400509132173,
+      "velocity" : 2.999999999999999,
+      "acceleration" : 2.6550284696523992E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 5.466553124523379,
+          "y" : -3.031584718256802
+        },
+        "rotation" : {
+          "radians" : -0.1272905672847317
+        }
+      },
+      "curvature" : 0.0726903757761293
+    }, {
+      "time" : 1.151792740021911,
+      "velocity" : 3.0,
+      "acceleration" : -8.698956559016603E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 5.566144844692476,
+          "y" : -3.0439633672617026
+        },
+        "rotation" : {
+          "radians" : -0.1200456934097773
+        }
+      },
+      "curvature" : 0.07168862870971174
+    }, {
+      "time" : 1.1858266464134202,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.558717775961129E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 5.66755561822356,
+          "y" : -3.0558215319688316
+        },
+        "rotation" : {
+          "radians" : -0.11277862194006867
+        }
+      },
+      "curvature" : 0.0706580941863296
+    }, {
+      "time" : 1.2204182150319014,
+      "velocity" : 3.0,
+      "acceleration" : -8.428702249529382E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 5.770712984037872,
+          "y" : -3.067124073879218
+        },
+        "rotation" : {
+          "radians" : -0.10550127152586215
+        }
+      },
+      "curvature" : 0.06959032685672162
+    }, {
+      "time" : 1.2555433700639118,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.308289645985856E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 5.875542325138333,
+          "y" : -3.0778384515395416
+        },
+        "rotation" : {
+          "radians" : -0.09822654372203397
+        }
+      },
+      "curvature" : 0.06847608142197704
+    }, {
+      "time" : 1.2911775963425318,
+      "velocity" : 3.0,
+      "acceleration" : -8.19691056891909E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 5.9819671467076425,
+          "y" : -3.0879349903634985
+        },
+        "rotation" : {
+          "radians" : -0.0909684258345216
+        }
+      },
+      "curvature" : 0.06730529458972327
+    }, {
+      "time" : 1.327296018124022,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.089909354206384,
+          "y" : -3.097387152453166
+        },
+        "rotation" : {
+          "radians" : -0.08374209711050397
+        }
+      },
+      "curvature" : 0.06606705425850384
+    }, {
+      "time" : 1.3638734752432662,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 7.999202988055673E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 6.199289531471132,
+          "y" : -3.1061718064203667
+        },
+        "rotation" : {
+          "radians" : -0.07656403878726785
+        }
+      },
+      "curvature" : 0.06474955745525926
+    }, {
+      "time" : 1.400884596685671,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.310027218812557,
+          "y" : -3.114269497208035
+        },
+        "rotation" : {
+          "radians" : -0.06945214847377139
+        }
+      },
+      "curvature" : 0.06334005821608486
+    }, {
+      "time" : 1.438303871650907,
+      "velocity" : 3.0,
+      "acceleration" : -7.831878599476166E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 6.422041191113526,
+          "y" : -3.1216647159115825
+        },
+        "rotation" : {
+          "radians" : -0.06242585929932353
+        }
+      },
+      "curvature" : 0.06182480635063325
+    }, {
+      "time" : 1.4761057182287096,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 7.758609490328921E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 6.535249735927209,
+          "y" : -3.1283461696002632
+        },
+        "rotation" : {
+          "radians" : -0.055506264223218596
+        }
+      },
+      "curvature" : 0.06018897785906844
+    }, {
+      "time" : 1.5142645498598688,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.6495709315751865,
+          "y" : -3.1343070511385376
+        },
+        "rotation" : {
+          "radians" : -0.04871624585462448
+        }
+      },
+      "curvature" : 0.05841659767581544
+    }, {
+      "time" : 1.5527548398175375,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.76492292524555,
+          "y" : -3.1395453090074383
+        },
+        "rotation" : {
+          "radians" : -0.0420806120796796
+        }
+      },
+      "curvature" : 0.05649045540029702
+    }, {
+      "time" : 1.5915511840162078,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.881224211091007,
+          "y" : -3.144063917125937
+        },
+        "rotation" : {
+          "radians" : -0.03562623772763141
+        }
+      },
+      "curvature" : 0.05439201475116482
+    }, {
+      "time" : 1.6306283625393438,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.998393908326989,
+          "y" : -3.147871144672308
+        },
+        "rotation" : {
+          "radians" : -0.029382212423449006
+        }
+      },
+      "curvature" : 0.05210131766377214
+    }, {
+      "time" : 1.6699614003730316,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 7.116352039329751,
+          "y" : -3.150980825905494
+        },
+        "rotation" : {
+          "radians" : -0.02337999466219851
+        }
+      },
+      "curvature" : 0.04959688426535946
+    }, {
+      "time" : 1.7095256279435234,
+      "velocity" : 3.0,
+      "acceleration" : -1.1557888191446533,
+      "pose" : {
+        "translation" : {
+          "x" : 7.235019807734481,
+          "y" : -3.1534126299864695
+        },
+        "rotation" : {
+          "radians" : -0.017653571989563004
+        }
+      },
+      "curvature" : 0.04685561044273053
+    }, {
+      "time" : 1.749606195370589,
+      "velocity" : 2.953675328302824,
+      "acceleration" : -4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 7.354319876533402,
+          "y" : -3.1551923307996104
+        },
+        "rotation" : {
+          "radians" : -0.012239626968958514
+        }
+      },
+      "curvature" : 0.04385266540863671
+    }, {
+      "time" : 1.791367879808199,
+      "velocity" : 2.786628590552384,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 7.4741766461738735,
+          "y" : -3.1563520767740556
+        },
+        "rotation" : {
+          "radians" : -0.007177708340391282
+        }
+      },
+      "curvature" : 0.040561392634885035
+    }, {
+      "time" : 1.8359816679429726,
+      "velocity" : 2.6081734380132895,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 7.594516532656503,
+          "y" : -3.156930660705073
+        },
+        "rotation" : {
+          "radians" : -0.002510406406084059
+        }
+      },
+      "curvature" : 0.036953218827764284
+    }, {
+      "time" : 1.8840509532237966,
+      "velocity" : 2.415896296889994,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 7.715268245633247,
+          "y" : -3.1569737895754244
+        },
+        "rotation" : {
+          "radians" : 0.0017164688167075213
+        }
+      },
+      "curvature" : 0.03299757736886057
+    }, {
+      "time" : 1.9364483118057918,
+      "velocity" : 2.206306862562013,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 7.836363066505513,
+          "y" : -3.156534354376733
+        },
+        "rotation" : {
+          "radians" : 0.005453708791556059
+        }
+      },
+      "curvature" : 0.028661854946767117
+    }, {
+      "time" : 1.9945178704913586,
+      "velocity" : 1.9740286278197459,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 7.957735126522266,
+          "y" : -3.1556726999308466
+        },
+        "rotation" : {
+          "radians" : 0.008648529987311705
+        }
+      },
+      "curvature" : 0.023911373099727667
+    }, {
+      "time" : 2.060528833894078,
+      "velocity" : 1.7099847742088685,
+      "acceleration" : -3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 8.079321684878137,
+          "y" : -3.1544568947112026
+        },
+        "rotation" : {
+          "radians" : 0.011244403839107409
+        }
+      },
+      "curvature" : 0.018709420234176016
+    }, {
+      "time" : 2.1389153715933484,
+      "velocity" : 1.3964386234117863,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 8.201063406811521,
+          "y" : -3.1529630006641938
+        },
+        "rotation" : {
+          "radians" : 0.013180898713490029
+        }
+      },
+      "curvature" : 0.013017354553867993
+    }, {
+      "time" : 2.2411422808075967,
+      "velocity" : 0.9875309865547927,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 8.322904641702685,
+          "y" : -3.1512753430305347
+        },
+        "rotation" : {
+          "radians" : 0.01439354131558399
+        }
+      },
+      "curvature" : 0.0067948044075592335
+    }, {
+      "time" : 2.488025027446295,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 8.444793701171875,
+          "y" : -3.149486780166626
+        },
+        "rotation" : {
+          "radians" : 0.014813707347434221
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 3.2910027503967285, 1.676727533340454, 0.0 ],
+      "y" : [ -2.5561599731445312, -0.49745607376098633, 0.0 ]
+    }, {
+      "x" : [ 8.444793701171875, 7.801394462585449, 0.0 ],
+      "y" : [ -3.149486780166626, 0.11557602882385254, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.0
+      }
+    }, {
+      "time" : 2.3999999463558197,
+      "rotation" : {
+        "radians" : 0.0
+      }
+    } ],
+    "reversed" : false,
+    "color" : 88.23529,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
   } ]
 }
\ No newline at end of file
diff --git a/src/main/deploy/autos/red/buddyautoleftsteal.json b/src/main/deploy/autos/red/buddyautoleftsteal.json
new file mode 100644
index 00000000..5264679f
--- /dev/null
+++ b/src/main/deploy/autos/red/buddyautoleftsteal.json
@@ -0,0 +1,3806 @@
+{
+  "autonomousSteps" : [ {
+    "type" : "script",
+    "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON\nsleep 1500",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "OPEN" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "INTAKE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "ON" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "sleep",
+        "args" : [ "1500" ],
+        "argTypes" : [ "long" ],
+        "reflection" : false
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 6.979461669921875,
+          "y" : 0.4386838674545288
+        },
+        "rotation" : {
+          "radians" : 2.4670512351406453
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.2218186293424793,
+      "velocity" : 0.8872745173699175,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 6.902607691762569,
+          "y" : 0.5001444677761242
+        },
+        "rotation" : {
+          "radians" : 2.4669815013720786
+        }
+      },
+      "curvature" : -0.001770057444385252
+    }, {
+      "time" : 0.3128823124020749,
+      "velocity" : 1.2515292496083,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 6.826564423769014,
+          "y" : 0.5609803238967288
+        },
+        "rotation" : {
+          "radians" : 2.46664012448608
+        }
+      },
+      "curvature" : -0.005584153832447457
+    }, {
+      "time" : 0.38171187830481973,
+      "velocity" : 1.526847513219279,
+      "acceleration" : 3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 6.751933529499183,
+          "y" : 0.6207539377647322
+        },
+        "rotation" : {
+          "radians" : 2.465845563577437
+        }
+      },
+      "curvature" : -0.011384177747344891
+    }, {
+      "time" : 0.4386276460816924,
+      "velocity" : 1.7545105843267694,
+      "acceleration" : 3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 6.679111945442855,
+          "y" : 0.6792085023480467
+        },
+        "rotation" : {
+          "radians" : 2.4644380166116457
+        }
+      },
+      "curvature" : -0.019103896941460693
+    }, {
+      "time" : 0.4876979714419739,
+      "velocity" : 1.9507918857678952,
+      "acceleration" : 3.9999999999999925,
+      "pose" : {
+        "translation" : {
+          "x" : 6.608317537175935,
+          "y" : 0.7362419663763262
+        },
+        "rotation" : {
+          "radians" : 2.4622863748597443
+        }
+      },
+      "curvature" : -0.028529142584546568
+    }, {
+      "time" : 0.5310864114411233,
+      "velocity" : 2.1243456457644925,
+      "acceleration" : 4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 6.539614755514776,
+          "y" : 0.7918810990831844
+        },
+        "rotation" : {
+          "radians" : 2.459302059694302
+        }
+      },
+      "curvature" : -0.03918060333359783
+    }, {
+      "time" : 0.5701494584010045,
+      "velocity" : 2.2805978336040176,
+      "acceleration" : -0.47832492111012465,
+      "pose" : {
+        "translation" : {
+          "x" : 6.472940292670501,
+          "y" : 0.8462555549484136
+        },
+        "rotation" : {
+          "radians" : 2.455456663103906
+        }
+      },
+      "curvature" : -0.05024047921606022
+    }, {
+      "time" : 0.6070915310824326,
+      "velocity" : 2.262927519603029,
+      "acceleration" : -4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 6.40812873840332,
+          "y" : 0.8995719384402037
+        },
+        "rotation" : {
+          "radians" : 2.4507991475637327
+        }
+      },
+      "curvature" : -0.06056066057312981
+    }, {
+      "time" : 0.6446468880225168,
+      "velocity" : 2.1127060918426923,
+      "acceleration" : -4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 6.3449382361768585,
+          "y" : 0.95208786875736
+        },
+        "rotation" : {
+          "radians" : 2.445467378655361
+        }
+      },
+      "curvature" : -0.06878202666638442
+    }, {
+      "time" : 0.6843932978096723,
+      "velocity" : 1.9537204526940704,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.28307613931247,
+          "y" : 1.0040860445715225
+        },
+        "rotation" : {
+          "radians" : 2.4396891056204377
+        }
+      },
+      "curvature" : -0.07355656872184071
+    }, {
+      "time" : 0.7271558960672502,
+      "velocity" : 1.782670059663759,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.222224667143564,
+          "y" : 1.0558483087693844
+        },
+        "rotation" : {
+          "radians" : 2.4337699158796475
+        }
+      },
+      "curvature" : -0.07380896893317555
+    }, {
+      "time" : 0.7741602914447601,
+      "velocity" : 1.5946524781537195,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.162066561169922,
+          "y" : 1.1076297131949104
+        },
+        "rotation" : {
+          "radians" : 2.428069867169106
+        }
+      },
+      "curvature" : -0.06893533742465202
+    }, {
+      "time" : 0.8273894003721558,
+      "velocity" : 1.3817360424441365,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 6.1023107412120225,
+          "y" : 1.1596325833915557
+        },
+        "rotation" : {
+          "radians" : 2.422974921973857
+        }
+      },
+      "curvature" : -0.058846523386872546
+    }, {
+      "time" : 0.8905738529525908,
+      "velocity" : 1.1289982321223961,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 6.042717961565359,
+          "y" : 1.2119805833444843
+        },
+        "rotation" : {
+          "radians" : 2.418872019409643
+        }
+      },
+      "curvature" : -0.043821739722789785
+    }, {
+      "time" : 0.9731110220523553,
+      "velocity" : 0.7988495557233383,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.9831264671547615,
+          "y" : 1.2646927802227879
+        },
+        "rotation" : {
+          "radians" : 2.4161366104589406
+        }
+      },
+      "curvature" : -0.02420493001126614
+    }, {
+      "time" : 1.1728234109831899,
+      "velocity" : 0.0,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.923477649688721,
+          "y" : 1.317657709121704
+        },
+        "rotation" : {
+          "radians" : 2.4151395686141304
+        }
+      },
+      "curvature" : -0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 6.979461669921875, -1.232039451599121, 0.0 ],
+      "y" : [ 0.4386838674545288, 0.9852284789085388, 0.0 ]
+    }, {
+      "x" : [ 5.923477649688721, -0.9546575546264648, 0.0 ],
+      "y" : [ 1.317657709121704, 0.8482611179351807, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.7759904509715376
+      }
+    }, {
+      "time" : 1.0799999758601189,
+      "rotation" : {
+        "radians" : 2.3681701999637936
+      }
+    } ],
+    "reversed" : false,
+    "color" : 209.88235,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "VisionManager.shootBalls 13",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.VisionManager.shootBalls",
+        "args" : [ "13" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.923477649688721,
+          "y" : 1.317657709121704
+        },
+        "rotation" : {
+          "radians" : 2.4280629259945457
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.1916862579332135,
+      "velocity" : 0.766745031732854,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.8679256060540865,
+          "y" : 1.3657653522359396
+        },
+        "rotation" : {
+          "radians" : 2.427546282399253
+        }
+      },
+      "curvature" : -0.013552982296289647
+    }, {
+      "time" : 0.26883282720714063,
+      "velocity" : 1.0753313088285628,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.814256962185027,
+          "y" : 1.4123322981758974
+        },
+        "rotation" : {
+          "radians" : 2.426170125310746
+        }
+      },
+      "curvature" : -0.025078420554477936
+    }, {
+      "time" : 0.3253598004362359,
+      "velocity" : 1.3014392017449439,
+      "acceleration" : 3.999999999999993,
+      "pose" : {
+        "translation" : {
+          "x" : 5.763593902279354,
+          "y" : 1.4564440353524333
+        },
+        "rotation" : {
+          "radians" : 2.424133609238276
+        }
+      },
+      "curvature" : -0.035413495196781865
+    }, {
+      "time" : 0.37047014804781875,
+      "velocity" : 1.4818805921912748,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.716341427527368,
+          "y" : 1.4977757879532874
+        },
+        "rotation" : {
+          "radians" : 2.4216494598565745
+        }
+      },
+      "curvature" : -0.043163158740255655
+    }, {
+      "time" : 0.44071473447231035,
+      "velocity" : 1.7628589378892412,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.630849667446455,
+          "y" : 1.57313263497781
+        },
+        "rotation" : {
+          "radians" : 2.4166846345884783
+        }
+      },
+      "curvature" : -0.03766210226111181
+    }, {
+      "time" : 0.4973489273055233,
+      "velocity" : 1.989395709222093,
+      "acceleration" : -1.536593152344523,
+      "pose" : {
+        "translation" : {
+          "x" : 5.551411211490631,
+          "y" : 1.643696241080761
+        },
+        "rotation" : {
+          "radians" : 2.4146239525620357
+        }
+      },
+      "curvature" : 0.002386794122151864
+    }, {
+      "time" : 0.5545817819308377,
+      "velocity" : 1.9014520967157054,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 5.468152332963655,
+          "y" : 1.7176221262780018
+        },
+        "rotation" : {
+          "radians" : 2.416957846275337
+        }
+      },
+      "curvature" : 0.03338570540861069
+    }, {
+      "time" : 0.5881329413084252,
+      "velocity" : 1.7672474592053555,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.422027663326844,
+          "y" : 1.7583682663707805
+        },
+        "rotation" : {
+          "radians" : 2.419124204295016
+        }
+      },
+      "curvature" : 0.03568476607637792
+    }, {
+      "time" : 0.6278550666121283,
+      "velocity" : 1.6083589579905428,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.3716818848624825,
+          "y" : 1.8026411016471684
+        },
+        "rotation" : {
+          "radians" : 2.42141292546337
+        }
+      },
+      "curvature" : 0.03192840803852446
+    }, {
+      "time" : 0.67622301229786,
+      "velocity" : 1.4148871752476162,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.316670433480795,
+          "y" : 1.850801377645439
+        },
+        "rotation" : {
+          "radians" : 2.4235110036015075
+        }
+      },
+      "curvature" : 0.025280758123603746
+    }, {
+      "time" : 0.7373987455586821,
+      "velocity" : 1.1701842422043276,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.257077135815052,
+          "y" : 1.9027724422048777
+        },
+        "rotation" : {
+          "radians" : 2.4252082067772336
+        }
+      },
+      "curvature" : 0.017656926445273908
+    }, {
+      "time" : 0.8212616808414924,
+      "velocity" : 0.8347325010730865,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.193638766594631,
+          "y" : 1.9579375321181942
+        },
+        "rotation" : {
+          "radians" : 2.4263532239110557
+        }
+      },
+      "curvature" : 0.009537673600406624
+    }, {
+      "time" : 1.029944806109764,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.127869606018066,
+          "y" : 2.0150370597839355
+        },
+        "rotation" : {
+          "radians" : 2.4267848465547592
+        }
+      },
+      "curvature" : -0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.923477649688721, -0.8946475982666016, 0.0 ],
+      "y" : [ 1.317657709121704, 0.7744828462600708, 0.0 ]
+    }, {
+      "x" : [ 5.127869606018066, -1.0593681335449219, 0.0 ],
+      "y" : [ 2.0150370597839355, 0.9194501638412476, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.356194490192345
+      }
+    }, {
+      "time" : 0.9399999789893627,
+      "rotation" : {
+        "radians" : 2.4532338595176353
+      }
+    } ],
+    "reversed" : false,
+    "color" : 154.35294,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "VisionManager.shootBalls 8\nDrive.turnToAngle 40",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.VisionManager.shootBalls",
+        "args" : [ "8" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Drive.turnToAngle",
+        "args" : [ "40" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.127869606018066,
+          "y" : 2.0150370597839355
+        },
+        "rotation" : {
+          "radians" : 0.9676638940223135
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.23907872328411964,
+      "velocity" : 0.9563148931364785,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.192705269680573,
+          "y" : 2.109189995806446
+        },
+        "rotation" : {
+          "radians" : 0.9679091836027879
+        }
+      },
+      "curvature" : 0.004044048851551997
+    }, {
+      "time" : 0.3327971604973431,
+      "velocity" : 1.3311886419893724,
+      "acceleration" : 3.9999999999999907,
+      "pose" : {
+        "translation" : {
+          "x" : 5.253460748921498,
+          "y" : 2.197499692440033
+        },
+        "rotation" : {
+          "radians" : 0.9685084969782757
+        }
+      },
+      "curvature" : 0.007076805090390186
+    }, {
+      "time" : 0.39816082847441153,
+      "velocity" : 1.5926433138976455,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.307566488200337,
+          "y" : 2.2762623392827663
+        },
+        "rotation" : {
+          "radians" : 0.9692958679676649
+        }
+      },
+      "curvature" : 0.009223838838357203
+    }, {
+      "time" : 0.44663324990958686,
+      "velocity" : 1.7865329996383468,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 5.353885251563042,
+          "y" : 2.343804362230003
+        },
+        "rotation" : {
+          "radians" : 0.9700562639818262
+        }
+      },
+      "curvature" : 0.008519844053710474
+    }, {
+      "time" : 0.5116917209128728,
+      "velocity" : 2.0467668836514905,
+      "acceleration" : 1.992648464590459,
+      "pose" : {
+        "translation" : {
+          "x" : 5.424351285982993,
+          "y" : 2.4466790923615918
+        },
+        "rotation" : {
+          "radians" : 0.9696596521756633
+        }
+      },
+      "curvature" : -0.030593782175814987
+    }, {
+      "time" : 0.5546990819250267,
+      "velocity" : 2.1324654355384465,
+      "acceleration" : -3.9999999999999925,
+      "pose" : {
+        "translation" : {
+          "x" : 5.475393638014793,
+          "y" : 2.5206459313631058
+        },
+        "rotation" : {
+          "radians" : 0.9621486944770179
+        }
+      },
+      "curvature" : -0.13823598332150094
+    }, {
+      "time" : 0.5969447671310003,
+      "velocity" : 1.9634826947145523,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.5252832076948835,
+          "y" : 2.591331197647378
+        },
+        "rotation" : {
+          "radians" : 0.9510157851137995
+        }
+      },
+      "curvature" : -0.09101992339026437
+    }, {
+      "time" : 0.6595418386244202,
+      "velocity" : 1.7130944087408728,
+      "acceleration" : -3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 5.592436509672552,
+          "y" : 2.6847757240757346
+        },
+        "rotation" : {
+          "radians" : 0.9456880198624937
+        }
+      },
+      "curvature" : -0.018285197310074868
+    }, {
+      "time" : 0.7058887043411002,
+      "velocity" : 1.527706945874153,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.636414063088068,
+          "y" : 2.745653131905783
+        },
+        "rotation" : {
+          "radians" : 0.9448592549816716
+        }
+      },
+      "curvature" : -0.005896965233362419
+    }, {
+      "time" : 0.768439061104125,
+      "velocity" : 1.2775055188220539,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 5.687825173619785,
+          "y" : 2.8167451025219634
+        },
+        "rotation" : {
+          "radians" : 0.9445964731587643
+        }
+      },
+      "curvature" : -0.001026058794484897
+    }, {
+      "time" : 0.8582844674175891,
+      "velocity" : 0.9181238935681972,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.745632715359989,
+          "y" : 2.896663040502972
+        },
+        "rotation" : {
+          "radians" : 0.9445853727793603
+        }
+      },
+      "curvature" : 3.9777751022351305E-4
+    }, {
+      "time" : 1.0878154408096383,
+      "velocity" : 0.0,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.807385444641113,
+          "y" : 2.9820399284362793
+        },
+        "rotation" : {
+          "radians" : 0.9446200461231519
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.127869606018066, 1.0498194694519043, 0.0 ],
+      "y" : [ 2.0150370597839355, 1.5242493152618408, 0.0 ]
+    }, {
+      "x" : [ 5.807385444641113, 1.0001163482666016, 0.0 ],
+      "y" : [ 2.9820399284362793, 1.3827617168426514, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.9380478872771981
+      }
+    }, {
+      "time" : 0.9999999776482582,
+      "rotation" : {
+        "radians" : 0.9272952180016122
+      }
+    } ],
+    "reversed" : false,
+    "color" : 59.52942,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@time 1.4\nIntake.setWantedIntakeState EJECT",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 1.4,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "EJECT" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.807385444641113,
+          "y" : 2.9820399284362793
+        },
+        "rotation" : {
+          "radians" : -3.1077984471976654
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.2180776573035808,
+      "velocity" : 0.8723106292143229,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.712328077295783,
+          "y" : 2.978708433225936
+        },
+        "rotation" : {
+          "radians" : -3.1041624869647366
+        }
+      },
+      "curvature" : 0.07273634738038368
+    }, {
+      "time" : 0.3111557355200276,
+      "velocity" : 1.2446229420801103,
+      "acceleration" : 3.9999999999999956,
+      "pose" : {
+        "translation" : {
+          "x" : 5.613894489667587,
+          "y" : 2.974581701005377
+        },
+        "rotation" : {
+          "radians" : -3.094472230094453
+        }
+      },
+      "curvature" : 0.11857740286674891
+    }, {
+      "time" : 0.38620859133114593,
+      "velocity" : 1.5448343653245833,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.509367328174033,
+          "y" : 2.9689573476464197
+        },
+        "rotation" : {
+          "radians" : -3.0808888420166256
+        }
+      },
+      "curvature" : 0.1367542913886183
+    }, {
+      "time" : 0.4534555009735825,
+      "velocity" : 1.8138220038943296,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 5.39670240160558,
+          "y" : 2.961229747164907
+        },
+        "rotation" : {
+          "radians" : -3.065345218180051
+        }
+      },
+      "curvature" : 0.13629877278960334
+    }, {
+      "time" : 0.5166742966413302,
+      "velocity" : 2.0666971865653205,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 5.274478519695425,
+          "y" : 2.950885042220694
+        },
+        "rotation" : {
+          "radians" : -3.0491647636938963
+        }
+      },
+      "curvature" : 0.1267933963841827
+    }, {
+      "time" : 0.5473532125193565,
+      "velocity" : 2.189412850077426,
+      "acceleration" : 4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 5.209495855934646,
+          "y" : 2.9445945602601076
+        },
+        "rotation" : {
+          "radians" : -3.041082387571697
+        }
+      },
+      "curvature" : 0.12079034959194605
+    }, {
+      "time" : 0.5775859258575893,
+      "velocity" : 2.310343703430357,
+      "acceleration" : 3.999999999999994,
+      "pose" : {
+        "translation" : {
+          "x" : 5.141847331689291,
+          "y" : 2.9374961546176337
+        },
+        "rotation" : {
+          "radians" : -3.0330761640829413
+        }
+      },
+      "curvature" : 0.11467075614396673
+    }, {
+      "time" : 0.607452738767687,
+      "velocity" : 2.429810955070748,
+      "acceleration" : 4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 5.071508179679264,
+          "y" : 2.929549181170158
+        },
+        "rotation" : {
+          "radians" : -3.025171418955131
+        }
+      },
+      "curvature" : 0.10876301194249897
+    }, {
+      "time" : 0.6370065753804001,
+      "velocity" : 2.5480263015216003,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.998483164915225,
+          "y" : 2.920717795803565
+        },
+        "rotation" : {
+          "radians" : -3.017377231431235
+        }
+      },
+      "curvature" : 0.10327528635802412
+    }, {
+      "time" : 0.6662799728523379,
+      "velocity" : 2.6651198914093515,
+      "acceleration" : 3.9999999999999942,
+      "pose" : {
+        "translation" : {
+          "x" : 4.922805017153899,
+          "y" : 2.910970798490861
+        },
+        "rotation" : {
+          "radians" : -3.0096904819137515
+        }
+      },
+      "curvature" : 0.09832969802645626
+    }, {
+      "time" : 0.6952901141770801,
+      "velocity" : 2.7811604567083203,
+      "acceleration" : 3.9999999999999836,
+      "pose" : {
+        "translation" : {
+          "x" : 4.844532863353379,
+          "y" : 2.9002814773702994
+        },
+        "rotation" : {
+          "radians" : -3.0020990455222543
+        }
+      },
+      "curvature" : 0.09398961737092937
+    }, {
+      "time" : 0.724042537599304,
+      "velocity" : 2.8961701503972157,
+      "acceleration" : 3.6380002628705683,
+      "pose" : {
+        "translation" : {
+          "x" : 4.763750660128433,
+          "y" : 2.888627452823507
+        },
+        "rotation" : {
+          "radians" : -2.994584224283115
+        }
+      },
+      "curvature" : 0.09028031417064307
+    }, {
+      "time" : 0.7525829010133674,
+      "velocity" : 3.0,
+      "acceleration" : -1.0263248734009206E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.680565626205809,
+          "y" : 2.8759905215536072
+        },
+        "rotation" : {
+          "radians" : -2.9871225391208203
+        }
+      },
+      "curvature" : 0.08720396777763022
+    }, {
+      "time" : 0.7814294661611385,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.000209901006119E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 4.595106674879542,
+          "y" : 2.8623565006633434
+        },
+        "rotation" : {
+          "radians" : -2.979687003342993
+        }
+      },
+      "curvature" : 0.08475025940246926
+    }, {
+      "time" : 0.8110292004704637,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.507522846466259,
+          "y" : 2.847715071733205
+        },
+        "rotation" : {
+          "radians" : -2.9722479862521185
+        }
+      },
+      "curvature" : 0.08290368133012417
+    }, {
+      "time" : 0.8413290035988551,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.417981740760487,
+          "y" : 2.832059624899553
+        },
+        "rotation" : {
+          "radians" : -2.9647737573971904
+        }
+      },
+      "curvature" : 0.08164850595350744
+    }, {
+      "time" : 0.872270133120958,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.326667949489956,
+          "y" : 2.8153871029327426
+        },
+        "rotation" : {
+          "radians" : -2.9572307833972085
+        }
+      },
+      "curvature" : 0.08097214961252051
+    }, {
+      "time" : 0.9037887421215142,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.233781488770907,
+          "y" : 2.7976978453152483
+        },
+        "rotation" : {
+          "radians" : -2.9495838324025407
+        }
+      },
+      "curvature" : 0.08086748247871706
+    }, {
+      "time" : 0.9358164189685482,
+      "velocity" : 3.0,
+      "acceleration" : -9.119536798471233E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.1395362315633975,
+          "y" : 2.778995432319789
+        },
+        "rotation" : {
+          "radians" : -2.941795926901371
+        }
+      },
+      "curvature" : 0.08133448893648988
+    }, {
+      "time" : 0.9682807293579756,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 9.019320323200669E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.044158340126607,
+          "y" : 2.7592865290874524
+        },
+        "rotation" : {
+          "radians" : -2.9338281737918814
+        }
+      },
+      "curvature" : 0.08238157300892571
+    }, {
+      "time" : 1.0011057608042824,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9478846984741445,
+          "y" : 2.73858072970582
+        },
+        "rotation" : {
+          "radians" : -2.925639491155511
+        }
+      },
+      "curvature" : 0.08402672524453093
+    }, {
+      "time" : 1.0342126698325704,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.850961344829351,
+          "y" : 2.7168904012870905
+        },
+        "rotation" : {
+          "radians" : -2.91718624359335
+        }
+      },
+      "curvature" : 0.08629871484426285
+    }, {
+      "time" : 1.0675202322086716,
+      "velocity" : 3.0,
+      "acceleration" : -0.1315579405706849,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7536419040806095,
+          "y" : 2.694230528046205
+        },
+        "rotation" : {
+          "radians" : -2.9084217919318283
+        }
+      },
+      "curvature" : 0.08923843792521573
+    }, {
+      "time" : 1.1009699296037385,
+      "velocity" : 2.9955994266979924,
+      "acceleration" : -0.16210586852590414,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6561860202366483,
+          "y" : 2.6706185553789727
+        },
+        "rotation" : {
+          "radians" : -2.899295958219869
+        }
+      },
+      "curvature" : 0.09290053504803716
+    }, {
+      "time" : 1.1345079603171326,
+      "velocity" : 2.990162715100549,
+      "acceleration" : -0.19242213116635215,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5588577888818484,
+          "y" : 2.646074233940194
+        },
+        "rotation" : {
+          "radians" : -2.889754402955238
+        }
+      },
+      "curvature" : 0.0973553849716613
+    }, {
+      "time" : 1.1680607870231765,
+      "velocity" : 2.983706408679117,
+      "acceleration" : -0.22706531667766747,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4619241896315485,
+          "y" : 2.620619463721785
+        },
+        "rotation" : {
+          "radians" : -2.8797379082054317
+        }
+      },
+      "curvature" : 0.10269158445623167
+    }, {
+      "time" : 1.2015549415534306,
+      "velocity" : 2.976101047873854,
+      "acceleration" : -0.2670814149888477,
+      "pose" : {
+        "translation" : {
+          "x" : 3.365653518587351,
+          "y" : 2.5942781381309032
+        },
+        "rotation" : {
+          "radians" : -2.869181557635527
+        }
+      },
+      "curvature" : 0.10901903383045562
+    }, {
+      "time" : 1.2349184127471249,
+      "velocity" : 2.9671902847785026,
+      "acceleration" : -0.31371465342134314,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2703138207924285,
+          "y" : 2.567075988068072
+        },
+        "rotation" : {
+          "radians" : -2.858013802441017
+        }
+      },
+      "curvature" : 0.11647276318602535
+    }, {
+      "time" : 1.2680813779246811,
+      "velocity" : 2.9567865766514014,
+      "acceleration" : -0.3684468103258239,
+      "pose" : {
+        "translation" : {
+          "x" : 3.1761713226868302,
+          "y" : 2.539040426005304
+        },
+        "rotation" : {
+          "radians" : -2.8461554009678793
+        }
+      },
+      "curvature" : 0.12521765233260732
+    }, {
+      "time" : 1.3009769666485926,
+      "velocity" : 2.944666301912286,
+      "acceleration" : -0.43303740193218687,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0834888645627863,
+          "y" : 2.510200390064227
+        },
+        "rotation" : {
+          "radians" : -2.833518219736519
+        }
+      },
+      "curvature" : 0.1354542158098308
+    }, {
+      "time" : 1.3335420599902073,
+      "velocity" : 2.930564398497954,
+      "acceleration" : -0.5095598286786093,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9925243330200146,
+          "y" : 2.4805861880942075
+        },
+        "rotation" : {
+          "radians" : -2.820003885293717
+        }
+      },
+      "curvature" : 0.14742563679773568
+    }, {
+      "time" : 1.365718127787587,
+      "velocity" : 2.91416876690357,
+      "acceleration" : -0.6004245148597813,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9035290934210276,
+          "y" : 2.4502293417504766
+        },
+        "rotation" : {
+          "radians" : -2.8055022808143084
+        }
+      },
+      "curvature" : 0.16142623105857898
+    }, {
+      "time" : 1.3974521047697568,
+      "velocity" : 2.8951149091694792,
+      "acceleration" : -0.7083738286934076,
+      "pose" : {
+        "translation" : {
+          "x" : 2.816746422346437,
+          "y" : 2.4191624305722534
+        },
+        "rotation" : {
+          "radians" : -2.7898898902549565
+        }
+      },
+      "curvature" : 0.17781148738953634
+    }, {
+      "time" : 1.4286973034286823,
+      "velocity" : 2.87298162816717,
+      "acceleration" : -0.8769242761272262,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7324099400502604,
+          "y" : 2.38741893606087
+        },
+        "rotation" : {
+          "radians" : -2.7730280085554613
+        }
+      },
+      "curvature" : 0.1970097361358105
+    }, {
+      "time" : 1.4594210697670953,
+      "velocity" : 2.846039211610955,
+      "acceleration" : -3.9999999999999947,
+      "pose" : {
+        "translation" : {
+          "x" : 2.650742042915226,
+          "y" : 2.3550330857578956
+        },
+        "rotation" : {
+          "radians" : -2.7547608625141673
+        }
+      },
+      "curvature" : 0.2195352951517829
+    }, {
+      "time" : 1.4900955256707504,
+      "velocity" : 2.723341387996335,
+      "acceleration" : -4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5719523359080814,
+          "y" : 2.3220396973232624
+        },
+        "rotation" : {
+          "radians" : -2.7349137288234022
+        }
+      },
+      "curvature" : 0.24600255746489014
+    }, {
+      "time" : 1.5212190951177167,
+      "velocity" : 2.5988471102084696,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4962360650348963,
+          "y" : 2.2884740226133893
+        },
+        "rotation" : {
+          "radians" : -2.7132912008461836
+        }
+      },
+      "curvature" : 0.2771397985751853
+    }, {
+      "time" : 1.5528031633912385,
+      "velocity" : 2.4725108371143825,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.42377254979637,
+          "y" : 2.254371591759306
+        },
+        "rotation" : {
+          "radians" : -2.689675854298644
+        }
+      },
+      "curvature" : 0.3138003172075707
+    }, {
+      "time" : 1.5848723080728448,
+      "velocity" : 2.3442342583879574,
+      "acceleration" : -3.9999999999999942,
+      "pose" : {
+        "translation" : {
+          "x" : 2.354723615643138,
+          "y" : 2.219768057244778
+        },
+        "rotation" : {
+          "radians" : -2.6638277072718006
+        }
+      },
+      "curvature" : 0.3569666341755263
+    }, {
+      "time" : 1.6174693044153108,
+      "velocity" : 2.213846273018094,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.289232026431076,
+          "y" : 2.184699037984432
+        },
+        "rotation" : {
+          "radians" : -2.6354850773300993
+        }
+      },
+      "curvature" : 0.4077405496577045
+    }, {
+      "time" : 1.6506622716676704,
+      "velocity" : 2.0810744040086555,
+      "acceleration" : -4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2274199168766096,
+          "y" : 2.1491999634018804
+        },
+        "rotation" : {
+          "radians" : -2.6043677221311743
+        }
+      },
+      "curvature" : 0.4673075916967568
+    }, {
+      "time" : 1.6845551653471331,
+      "velocity" : 1.9455028292908045,
+      "acceleration" : -3.7890145173659073,
+      "pose" : {
+        "translation" : {
+          "x" : 2.169387225012014,
+          "y" : 2.1133059175078435
+        },
+        "rotation" : {
+          "radians" : -2.5701835156594286
+        }
+      },
+      "curvature" : 0.5368586687945079
+    }, {
+      "time" : 1.7192335134624979,
+      "velocity" : 1.8141060648434189,
+      "acceleration" : -3.2696015880426708,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1152101246407256,
+          "y" : 2.077051482978277
+        },
+        "rotation" : {
+          "radians" : -2.532640340224557
+        }
+      },
+      "curvature" : 0.6174451066520994
+    }, {
+      "time" : 1.7907419934854965,
+      "velocity" : 1.5803018250017051,
+      "acceleration" : -2.726779669291407,
+      "pose" : {
+        "translation" : {
+          "x" : 2.018599167179449,
+          "y" : 2.003596336511297
+        },
+        "rotation" : {
+          "radians" : -2.446433526852606
+        }
+      },
+      "curvature" : 0.8136612252229765
+    }, {
+      "time" : 1.8275860774684607,
+      "velocity" : 1.479836125863293,
+      "acceleration" : -2.359563803315625,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9761847286498835,
+          "y" : 1.9664608799550876
+        },
+        "rotation" : {
+          "radians" : -2.397408828393692
+        }
+      },
+      "curvature" : 0.9278899124682813
+    }, {
+      "time" : 1.8649660152071963,
+      "velocity" : 1.391635777804781,
+      "acceleration" : -1.9838468623386898,
+      "pose" : {
+        "translation" : {
+          "x" : 1.937661583645081,
+          "y" : 1.9290952336820055
+        },
+        "rotation" : {
+          "radians" : -2.3443966957035403
+        }
+      },
+      "curvature" : 1.0492344280773838
+    }, {
+      "time" : 1.902729666191217,
+      "velocity" : 1.3167184772896783,
+      "acceleration" : -1.5953761316272526,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9029635716538653,
+          "y" : 1.891529134866047
+        },
+        "rotation" : {
+          "radians" : -2.2876080290002703
+        }
+      },
+      "curvature" : 1.1720276125742721
+    }, {
+      "time" : 1.940679658288589,
+      "velocity" : 1.256173965702088,
+      "acceleration" : -1.186040735740344,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8719913626680516,
+          "y" : 1.85379088381519
+        },
+        "rotation" : {
+          "radians" : -2.227526882083615
+        }
+      },
+      "curvature" : 1.2877279640741648
+    }, {
+      "time" : 1.9785675742743187,
+      "velocity" : 1.2112373539507049,
+      "acceleration" : -0.7410587042667358,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8446108896377584,
+          "y" : 1.8159071880495183
+        },
+        "rotation" : {
+          "radians" : -2.164970018044204
+        }
+      },
+      "curvature" : 1.3850491721536529
+    }, {
+      "time" : 2.0160893592543716,
+      "velocity" : 1.1834315085916118,
+      "acceleration" : -0.23332707042705494,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8206517809267098,
+          "y" : 1.777903006379347
+        },
+        "rotation" : {
+          "radians" : -2.1011212361539733
+        }
+      },
+      "curvature" : 1.4508998888713995
+    }, {
+      "time" : 2.052881886984702,
+      "velocity" : 1.1748468158826877,
+      "acceleration" : 0.3888458638588137,
+      "pose" : {
+        "translation" : {
+          "x" : 1.799905792767543,
+          "y" : 1.7398013929833471
+        },
+        "rotation" : {
+          "radians" : -2.037523308037849
+        }
+      },
+      "curvature" : 1.4721810231732262
+    }, {
+      "time" : 2.088519327343306,
+      "velocity" : 1.188704287164646,
+      "acceleration" : 1.231210997720917,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7821252417171145,
+          "y" : 1.7016233414866697
+        },
+        "rotation" : {
+          "radians" : -1.976018179939325
+        }
+      },
+      "curvature" : 1.4380568182608686
+    }, {
+      "time" : 2.1225056544060728,
+      "velocity" : 1.2305486268164643,
+      "acceleration" : 2.534651579871874,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7670214371118043,
+          "y" : 1.6633876290390703
+        },
+        "rotation" : {
+          "radians" : -1.918641555195968
+        }
+      },
+      "curvature" : 1.341918509745125
+    }, {
+      "time" : 2.1542554976740123,
+      "velocity" : 1.311023417216232,
+      "acceleration" : 3.9999999999999845,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7542631135228248,
+          "y" : 1.6251106603930339
+        },
+        "rotation" : {
+          "radians" : -1.8674949603224105
+        }
+      },
+      "curvature" : 1.182232258965127
+    }, {
+      "time" : 2.2099359231623428,
+      "velocity" : 1.5337451191695526,
+      "acceleration" : 4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7342355685846904,
+          "y" : 1.5484857759979853
+        },
+        "rotation" : {
+          "radians" : -1.7919535583318533
+        }
+      },
+      "curvature" : 0.6868485079079413
+    }, {
+      "time" : 2.2579553489086845,
+      "velocity" : 1.7258228221549206,
+      "acceleration" : -1.2499750022631253,
+      "pose" : {
+        "translation" : {
+          "x" : 1.718481421470642,
+          "y" : 1.4718265533447266
+        },
+        "rotation" : {
+          "radians" : -1.7639859651971548
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 2.3039131087144136,
+      "velocity" : 1.6683767712377462,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7040171629614953,
+          "y" : 1.3951845909212786
+        },
+        "rotation" : {
+          "radians" : -1.744243059159777
+        }
+      },
+      "curvature" : 0.4949885871870906
+    }, {
+      "time" : 2.353201866781118,
+      "velocity" : 1.4712217389709286,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 1.692568655125001,
+          "y" : 1.3186628076501847
+        },
+        "rotation" : {
+          "radians" : -1.688734274061912
+        }
+      },
+      "curvature" : 0.9318135483719395
+    }, {
+      "time" : 2.409519830422982,
+      "velocity" : 1.2459498844034724,
+      "acceleration" : -2.1690899330402935,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6866636401594342,
+          "y" : 1.2423782268174186
+        },
+        "rotation" : {
+          "radians" : -1.6025452180114088
+        }
+      },
+      "curvature" : 1.3089484866875387
+    }, {
+      "time" : 2.4408900706331065,
+      "velocity" : 1.1779050121666355,
+      "acceleration" : -1.4371594032530797,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6864417690519602,
+          "y" : 1.204360419059464
+        },
+        "rotation" : {
+          "radians" : -1.5497323811772583
+        }
+      },
+      "curvature" : 1.4645464958895065
+    }, {
+      "time" : 2.4737794652080054,
+      "velocity" : 1.130637709486019,
+      "acceleration" : -0.882454393290906,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6883282148664875,
+          "y" : 1.1664440317872504
+        },
+        "rotation" : {
+          "radians" : -1.4916448185226532
+        }
+      },
+      "curvature" : 1.589559435549557
+    }, {
+      "time" : 2.5078700922418036,
+      "velocity" : 1.100554285890002,
+      "acceleration" : -0.4285512346859183,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6925034029607406,
+          "y" : 1.1286425412588699
+        },
+        "rotation" : {
+          "radians" : -1.4293799219170706
+        }
+      },
+      "curvature" : 1.677647696189635
+    }, {
+      "time" : 2.542863859363714,
+      "velocity" : 1.0855576637835957,
+      "acceleration" : -0.03409957451987376,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6991206308842841,
+          "y" : 1.0909691291280303
+        },
+        "rotation" : {
+          "radians" : -1.3641679929709554
+        }
+      },
+      "curvature" : 1.7243201694504038
+    }, {
+      "time" : 2.578478719165117,
+      "velocity" : 1.084343212217783,
+      "acceleration" : 0.32480160779729494,
+      "pose" : {
+        "translation" : {
+          "x" : 1.708307124635804,
+          "y" : 1.0534366687917367
+        },
+        "rotation" : {
+          "radians" : -1.2973160315953416
+        }
+      },
+      "curvature" : 1.72818476876106
+    }, {
+      "time" : 2.6144494673900835,
+      "velocity" : 1.0960265690749238,
+      "acceleration" : 0.662753960787673,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7201650949203895,
+          "y" : 1.0160577117379717
+        },
+        "rotation" : {
+          "radians" : -1.2301287631472986
+        }
+      },
+      "curvature" : 1.691537146255751
+    }, {
+      "time" : 2.6505308994153998,
+      "velocity" : 1.1199396810605933,
+      "acceleration" : 0.9891536088281452,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7347727934068158,
+          "y" : 0.9788444738933771
+        },
+        "rotation" : {
+          "radians" : -1.163822987935704
+        }
+      },
+      "curvature" : 1.6200724563638298
+    }, {
+      "time" : 2.686501528745972,
+      "velocity" : 1.1555201588747483,
+      "acceleration" : 1.310397118703277,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7521855689848245,
+          "y" : 0.941808821970934
+        },
+        "rotation" : {
+          "radians" : -1.0994531238813727
+        }
+      },
+      "curvature" : 1.52183879385189
+    }, {
+      "time" : 2.7221665450113437,
+      "velocity" : 1.202255493427397,
+      "acceleration" : 1.6311499809238765,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7724369240224052,
+          "y" : 0.9049622598176431
+        },
+        "rotation" : {
+          "radians" : -1.0378615567095117
+        }
+      },
+      "curvature" : 1.405821442899369
+    }, {
+      "time" : 2.757359258068479,
+      "velocity" : 1.2596600866592031,
+      "acceleration" : 1.9551274988150928,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7955395706230775,
+          "y" : 0.8683159147622064
+        },
+        "rotation" : {
+          "radians" : -0.9796592961829077
+        }
+      },
+      "curvature" : 1.2806102287854289
+    }, {
+      "time" : 2.7919408310448324,
+      "velocity" : 1.3272714709375524,
+      "acceleration" : 2.2856166223411245,
+      "pose" : {
+        "translation" : {
+          "x" : 1.821486486883173,
+          "y" : 0.8318805239627071
+        },
+        "rotation" : {
+          "radians" : -0.9252342480030827
+        }
+      },
+      "curvature" : 1.1534643716591353
+    }, {
+      "time" : 2.825798523286439,
+      "velocity" : 1.4046571751190786,
+      "acceleration" : 2.6258637992019844,
+      "pose" : {
+        "translation" : {
+          "x" : 1.850251973149117,
+          "y" : 0.7956664207542907
+        },
+        "rotation" : {
+          "radians" : -0.8747791350162617
+        }
+      },
+      "curvature" : 1.0298714520728465
+    }, {
+      "time" : 2.8588428851625496,
+      "velocity" : 1.4914271687372875,
+      "acceleration" : 2.9794041928936728,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8817927082747086,
+          "y" : 0.7596835209968459
+        },
+        "rotation" : {
+          "radians" : -0.8283296056482383
+        }
+      },
+      "curvature" : 0.9135232462092671
+    }, {
+      "time" : 2.891004389345329,
+      "velocity" : 1.5872492891492276,
+      "acceleration" : 3.5509647371659865,
+      "pose" : {
+        "translation" : {
+          "x" : 1.916048805878404,
+          "y" : 0.7239413094226848
+        },
+        "rotation" : {
+          "radians" : -0.7858044305587907
+        }
+      },
+      "curvature" : 0.8065539475387932
+    }, {
+      "time" : 2.9523592966906644,
+      "velocity" : 1.8051184015845994,
+      "acceleration" : 3.999999999999984,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9923910543609047,
+          "y" : 0.6532146522016653
+        },
+        "rotation" : {
+          "radians" : -0.7118327324730775
+        }
+      },
+      "curvature" : 0.6236089171423088
+    }, {
+      "time" : 3.0100353656283176,
+      "velocity" : 2.035822677335212,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0785084606141027,
+          "y" : 0.5835531856100715
+        },
+        "rotation" : {
+          "radians" : -0.6511181043270622
+        }
+      },
+      "curvature" : 0.4800010235812804
+    }, {
+      "time" : 3.0646190962309157,
+      "velocity" : 2.2541575997456036,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1734333233560115,
+          "y" : 0.5150158743770881
+        },
+        "rotation" : {
+          "radians" : -0.6017399678104041
+        }
+      },
+      "curvature" : 0.3691740752256128
+    }, {
+      "time" : 3.1166653761479624,
+      "velocity" : 2.46234271941379,
+      "acceleration" : 4.000000000000028,
+      "pose" : {
+        "translation" : {
+          "x" : 2.276034298238187,
+          "y" : 0.4476534745680283
+        },
+        "rotation" : {
+          "radians" : -0.5619346801385068
+        }
+      },
+      "curvature" : 0.2836399510576229
+    }, {
+      "time" : 3.1418275733370513,
+      "velocity" : 2.562991508170146,
+      "acceleration" : 4.0000000000000275,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3298233544980946,
+          "y" : 0.4144263756493385
+        },
+        "rotation" : {
+          "radians" : -0.5451464563946322
+        }
+      },
+      "curvature" : 0.24824311811855138
+    }, {
+      "time" : 3.1664400864835103,
+      "velocity" : 2.6614415607559834,
+      "acceleration" : 3.999999999999966,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3850501980787406,
+          "y" : 0.38150809671011743
+        },
+        "rotation" : {
+          "radians" : -0.5302189330250398
+        }
+      },
+      "curvature" : 0.21679631372622093
+    }, {
+      "time" : 3.1905125871491076,
+      "velocity" : 2.757731563418372,
+      "acceleration" : 4.000000000000013,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4415424379903916,
+          "y" : 0.3489024790262656
+        },
+        "rotation" : {
+          "radians" : -0.5170134828220253
+        }
+      },
+      "curvature" : 0.1886809876230122
+    }, {
+      "time" : 3.214049672205038,
+      "velocity" : 2.851879903642094,
+      "acceleration" : 3.303657668196632,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4991237930953503,
+          "y" : 0.31661276891827583
+        },
+        "rotation" : {
+          "radians" : -0.5054087566305369
+        }
+      },
+      "curvature" : 0.16335410873820064
+    }, {
+      "time" : 3.2371151553370474,
+      "velocity" : 2.928080363861817,
+      "acceleration" : 1.0968430910701312,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5576151483652376,
+          "y" : 0.2846416040989146
+        },
+        "rotation" : {
+          "radians" : -0.49529950641493536
+        }
+      },
+      "curvature" : 0.14033935814094875
+    }, {
+      "time" : 3.259949848690555,
+      "velocity" : 2.953126439503317,
+      "acceleration" : 1.0318522973125155,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6168356111382742,
+          "y" : 0.25299100002090213
+        },
+        "rotation" : {
+          "radians" : -0.48659537038691486
+        }
+      },
+      "curvature" : 0.11921786782590763
+    }, {
+      "time" : 3.2827100656165435,
+      "velocity" : 2.9766116216257297,
+      "acceleration" : 0.9835246105005508,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6766035673765614,
+          "y" : 0.22166233622459286
+        },
+        "rotation" : {
+          "radians" : -0.47921968759438144
+        }
+      },
+      "curvature" : 0.09961923590719055
+    }, {
+      "time" : 3.305354934644453,
+      "velocity" : 2.99888340761624,
+      "acceleration" : 0.04948814588093301,
+      "pose" : {
+        "translation" : {
+          "x" : 2.736737737923363,
+          "y" : 0.19065634268565645
+        },
+        "rotation" : {
+          "radians" : -0.4731083784610581
+        }
+      },
+      "curvature" : 0.08121320610983931
+    }, {
+      "time" : 3.3279177598197176,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.3150721678562816E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7970582347603883,
+          "y" : 0.15997308616275907
+        },
+        "rotation" : {
+          "radians" : -0.4682089061577221
+        }
+      },
+      "curvature" : 0.063702197976732
+    }, {
+      "time" : 3.3504305526891756,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.857387617265072,
+          "y" : 0.1296119565452436
+        },
+        "rotation" : {
+          "radians" : -0.4644793182582315
+        }
+      },
+      "curvature" : 0.0468147679013892
+    }, {
+      "time" : 3.3728462381053794,
+      "velocity" : 3.0,
+      "acceleration" : -1.3294976180711803E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9175519484678567,
+          "y" : 0.09957165320080996
+        },
+        "rotation" : {
+          "radians" : -0.461887356526069
+        }
+      },
+      "curvature" : 0.03030003960646033
+    }, {
+      "time" : 3.395114760380951,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.3412784420541495E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 2.977381851309474,
+          "y" : 0.06985017132319626
+        },
+        "rotation" : {
+          "radians" : -0.4604096131005984
+        }
+      },
+      "curvature" : 0.013923147393524328
+    }, {
+      "time" : 3.4171876919923743,
+      "velocity" : 3.0,
+      "acceleration" : -0.04926038986669166,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0367135648982266,
+          "y" : 0.04044478827985998
+        },
+        "rotation" : {
+          "radians" : -0.46003070242576016
+        }
+      },
+      "curvature" : -0.0025382237303508883
+    }, {
+      "time" : 3.43902255151928,
+      "velocity" : 2.99892440630702,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0953900007672708,
+          "y" : 0.011352049959658217
+        },
+        "rotation" : {
+          "radians" : -0.4607424088911207
+        }
+      },
+      "curvature" : -0.019296044170462233
+    }, {
+      "time" : 3.460894301999556,
+      "velocity" : 2.911437404385916,
+      "acceleration" : -4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 3.153261799131896,
+          "y" : -0.017432242879471982
+        },
+        "rotation" : {
+          "radians" : -0.46254275944377354
+        }
+      },
+      "curvature" : -0.03655263042494752
+    }, {
+      "time" : 3.483096236841796,
+      "velocity" : 2.8226296650169553,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2101883851468074,
+          "y" : -0.04591304826283249
+        },
+        "rotation" : {
+          "radians" : -0.4654349576602489
+        }
+      },
+      "curvature" : -0.05449941944261404
+    }, {
+      "time" : 3.5284559791470897,
+      "velocity" : 2.6411906957957814,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 3.320693882987083,
+          "y" : -0.10198790459356033
+        },
+        "rotation" : {
+          "radians" : -0.4745255802104961
+        }
+      },
+      "curvature" : -0.09314935868314721
+    }, {
+      "time" : 3.5750713755207792,
+      "velocity" : 2.454729110301023,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4259977320907637,
+          "y" : -0.1569278776878491
+        },
+        "rotation" : {
+          "radians" : -0.488085849209704
+        }
+      },
+      "curvature" : -0.13632788196047352
+    }, {
+      "time" : 3.6230020187291068,
+      "velocity" : 2.2630065374677133,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 3.525399329757196,
+          "y" : -0.21080134048127874
+        },
+        "rotation" : {
+          "radians" : -0.5061441065978821
+        }
+      },
+      "curvature" : -0.18426344503119224
+    }, {
+      "time" : 3.6724555712310893,
+      "velocity" : 2.065192327459783,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6184400330154176,
+          "y" : -0.2636901170639021
+        },
+        "rotation" : {
+          "radians" : -0.528561557547075
+        }
+      },
+      "curvature" : -0.2354062697850754
+    }, {
+      "time" : 3.723886359469485,
+      "velocity" : 1.8594691745062002,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7049369588571714,
+          "y" : -0.3156899195544618
+        },
+        "rotation" : {
+          "radians" : -0.5548405336431187
+        }
+      },
+      "curvature" : -0.28492058176951146
+    }, {
+      "time" : 3.7781787791469426,
+      "velocity" : 1.6422994957963697,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 3.785016784469917,
+          "y" : -0.36691078497460694
+        },
+        "rotation" : {
+          "radians" : -0.5838549927785415
+        }
+      },
+      "curvature" : -0.32275632767074536
+    }, {
+      "time" : 3.837038685288918,
+      "velocity" : 1.4068598712284692,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.859149547469844,
+          "y" : -0.41747751212311
+        },
+        "rotation" : {
+          "radians" : -0.6135313524400181
+        }
+      },
+      "curvature" : -0.3322823649480369
+    }, {
+      "time" : 3.9040276125177047,
+      "velocity" : 1.1389041623133211,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9281824461348833,
+          "y" : -0.46753009845008364
+        },
+        "rotation" : {
+          "radians" : -0.6405875389886441
+        }
+      },
+      "curvature" : -0.29180251710931904
+    }, {
+      "time" : 3.988546345848521,
+      "velocity" : 0.800829228990057,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9933736396377206,
+          "y" : -0.5172241769311974
+        },
+        "rotation" : {
+          "radians" : -0.6605456517130821
+        }
+      },
+      "curvature" : -0.1820147647738938
+    }, {
+      "time" : 4.188753653096035,
+      "velocity" : 0.0,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 4.056426048278809,
+          "y" : -0.5667314529418945
+        },
+        "rotation" : {
+          "radians" : -0.6682894149758353
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.807385444641113, -3.022507667541504, 0.0 ],
+      "y" : [ 2.9820399284362793, -0.10218214988708496, 0.0 ]
+    }, {
+      "x" : [ 1.718481421470642, -0.47991621494293213, 0.0 ],
+      "y" : [ 1.4718265533447266, -2.453189492225647, 0.0 ]
+    }, {
+      "x" : [ 4.056426048278809, 2.0053954124450684, 0.0 ],
+      "y" : [ -0.5667314529418945, -1.5832068920135498, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : -3.127725471555593
+      }
+    }, {
+      "time" : 2.249999949708581,
+      "rotation" : {
+        "radians" : -0.798910986801039
+      }
+    }, {
+      "time" : 4.099999908357859,
+      "rotation" : {
+        "radians" : -0.6659689899224528
+      }
+    } ],
+    "reversed" : false,
+    "color" : 192.47057,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@t 2\nIntake.setWantedIntakeState EJECT",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 2.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "EJECT" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "@t 2.7\nDrive.setAutoRotation 35",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 2.7,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Drive.setAutoRotation",
+        "args" : [ "35" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.056426048278809,
+          "y" : -0.5667314529418945
+        },
+        "rotation" : {
+          "radians" : 2.4520537478581654
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.18493870993194386,
+      "velocity" : 0.7397548397277754,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 4.003633991609843,
+          "y" : -0.5232320818440126
+        },
+        "rotation" : {
+          "radians" : 2.4530901867155106
+        }
+      },
+      "curvature" : 0.029752937013657187
+    }, {
+      "time" : 0.26148282578134485,
+      "velocity" : 1.0459313031253796,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9508028739061523,
+          "y" : -0.47987904225311695
+        },
+        "rotation" : {
+          "radians" : 2.4560494170073883
+        }
+      },
+      "curvature" : 0.05635458173870567
+    }, {
+      "time" : 0.32012947576533146,
+      "velocity" : 1.2805179030613263,
+      "acceleration" : 3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 3.897901180103951,
+          "y" : -0.4368061550801574
+        },
+        "rotation" : {
+          "radians" : 2.4607189045800832
+        }
+      },
+      "curvature" : 0.08009487969635327
+    }, {
+      "time" : 0.36946312936714043,
+      "velocity" : 1.477852517468562,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8449053191515077,
+          "y" : -0.39413398946635425
+        },
+        "rotation" : {
+          "radians" : 2.466900851787993
+        }
+      },
+      "curvature" : 0.10120927083782304
+    }, {
+      "time" : 0.4128042913826585,
+      "velocity" : 1.6512171655306345,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 3.791799285257279,
+          "y" : -0.35197029855173945
+        },
+        "rotation" : {
+          "radians" : 2.474409887161352
+        }
+      },
+      "curvature" : 0.1198860351486836
+    }, {
+      "time" : 0.451853631575856,
+      "velocity" : 1.8074145263034247,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.738574319138042,
+          "y" : -0.3104104552436979
+        },
+        "rotation" : {
+          "radians" : 2.483071079673296
+        }
+      },
+      "curvature" : 0.13627342891926963
+    }, {
+      "time" : 0.4876202915697968,
+      "velocity" : 1.9504811662791879,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6852285692670304,
+          "y" : -0.2695378879855088
+        },
+        "rotation" : {
+          "radians" : 2.4927182713379636
+        }
+      },
+      "curvature" : 0.15048631581735405
+    }, {
+      "time" : 0.5207612575253416,
+      "velocity" : 2.0830450301013674,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.631766753122065,
+          "y" : -0.2294245165248867
+        },
+        "rotation" : {
+          "radians" : 2.5031927075423317
+        }
+      },
+      "curvature" : 0.16261213661992346
+    }, {
+      "time" : 0.5517326643887606,
+      "velocity" : 2.206930657555043,
+      "acceleration" : 3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5781998184336885,
+          "y" : -0.1901311876825229
+        },
+        "rotation" : {
+          "radians" : 2.514341935981025
+        }
+      },
+      "curvature" : 0.1727161388917002
+    }, {
+      "time" : 0.5808665583284028,
+      "velocity" : 2.3234662333136122,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5245446044332986,
+          "y" : -0.15170811112062665
+        },
+        "rotation" : {
+          "radians" : 2.526018940159826
+        }
+      },
+      "curvature" : 0.18084582936508686
+    }, {
+      "time" : 0.6084135883193114,
+      "velocity" : 2.4336543532772463,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.470823503101282,
+          "y" : -0.11419529511146642
+        },
+        "rotation" : {
+          "radians" : 2.538081470866874
+        }
+      },
+      "curvature" : 0.18703463399877743
+    }, {
+      "time" : 0.6345684464707756,
+      "velocity" : 2.5382737858831033,
+      "acceleration" : 3.9999999999999862,
+      "pose" : {
+        "translation" : {
+          "x" : 3.417064120415148,
+          "y" : -0.07762298230591114
+        },
+        "rotation" : {
+          "radians" : 2.5503915380457647
+        }
+      },
+      "curvature" : 0.19130476386228212
+    }, {
+      "time" : 0.6594858696162618,
+      "velocity" : 2.6379434784650475,
+      "acceleration" : 3.999999999999993,
+      "pose" : {
+        "translation" : {
+          "x" : 3.36329893759766,
+          "y" : -0.04201208550197144
+        },
+        "rotation" : {
+          "radians" : 2.5628150257457696
+        }
+      },
+      "curvature" : 0.19366929489891171
+    }, {
+      "time" : 0.6832911528298411,
+      "velocity" : 2.733164611319365,
+      "acceleration" : 3.0356954614896483,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3095649723649707,
+          "y" : -0.007374623413340942
+        },
+        "rotation" : {
+          "radians" : 2.575221394062975
+        }
+      },
+      "curvature" : 0.1941334791085683
+    }, {
+      "time" : 0.7282945981321051,
+      "velocity" : 2.8697813659748452,
+      "acceleration" : 0.35602362885687006,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2023594154743478,
+          "y" : 0.05897377757355571
+        },
+        "rotation" : {
+          "radians" : 2.5994770448888582
+        }
+      },
+      "curvature" : 0.18934541322883708
+    }, {
+      "time" : 0.7712234425204774,
+      "velocity" : 2.8850650489366254,
+      "acceleration" : 0.5853616532790817,
+      "pose" : {
+        "translation" : {
+          "x" : 3.095821448769353,
+          "y" : 0.1214873934270031
+        },
+        "rotation" : {
+          "radians" : 2.622176713767235
+        }
+      },
+      "curvature" : 0.17683058489309955
+    }, {
+      "time" : 0.8129042620605731,
+      "velocity" : 2.909463402372643,
+      "acceleration" : 0.8532903307335525,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9903759750508243,
+          "y" : 0.18034581308165798
+        },
+        "rotation" : {
+          "radians" : 2.6423801962411266
+        }
+      },
+      "curvature" : 0.156321766875571
+    }, {
+      "time" : 0.8531469929208557,
+      "velocity" : 2.943802135498035,
+      "acceleration" : 0.5959644267737211,
+      "pose" : {
+        "translation" : {
+          "x" : 2.886487960834632,
+          "y" : 0.23582909704128951
+        },
+        "rotation" : {
+          "radians" : 2.659177929985617
+        }
+      },
+      "curvature" : 0.12732676169969878
+    }, {
+      "time" : 0.8919109189015986,
+      "velocity" : 2.966904056424647,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 2.784651596291951,
+          "y" : 0.28830383278545924
+        },
+        "rotation" : {
+          "radians" : 2.6716758173179893
+        }
+      },
+      "curvature" : 0.0890361198334152
+    }, {
+      "time" : 0.9303572438648844,
+      "velocity" : 2.813118756571504,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.68537945518953,
+          "y" : 0.338209190176201
+        },
+        "rotation" : {
+          "radians" : 2.6789757213651453
+        }
+      },
+      "curvature" : 0.04021801225466915
+    }, {
+      "time" : 0.9696416444039297,
+      "velocity" : 2.6559811544153225,
+      "acceleration" : -4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5891916548299605,
+          "y" : 0.3860429768647009
+        },
+        "rotation" : {
+          "radians" : 2.680153087048729
+        }
+      },
+      "curvature" : -0.02088202984869535
+    }, {
+      "time" : 1.0098343280634516,
+      "velocity" : 2.4952104197772345,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4966050159919497,
+          "y" : 0.4323476936979773
+        },
+        "rotation" : {
+          "radians" : 2.6742351029390625
+        }
+      },
+      "curvature" : -0.09660379275555367
+    }, {
+      "time" : 1.0510425689247183,
+      "velocity" : 2.3303774563321675,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4081222228705883,
+          "y" : 0.47769659012556076
+        },
+        "rotation" : {
+          "radians" : 2.660185719014542
+        }
+      },
+      "curvature" : -0.18983275500917693
+    }, {
+      "time" : 1.0934364510155294,
+      "velocity" : 2.160801927968923,
+      "acceleration" : -4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3242209830176215,
+          "y" : 0.5226797196061739
+        },
+        "rotation" : {
+          "radians" : 2.636908345303076
+        }
+      },
+      "curvature" : -0.30373535138801905
+    }, {
+      "time" : 1.1372916096013064,
+      "velocity" : 1.9853812936258146,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 2.245343187281719,
+          "y" : 0.5678899950144114
+        },
+        "rotation" : {
+          "radians" : 2.6032836790059384
+        }
+      },
+      "curvature" : -0.4410606681722093
+    }, {
+      "time" : 1.1830628813303603,
+      "velocity" : 1.8022962067095987,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.171884069748746,
+          "y" : 0.6139092440474201
+        },
+        "rotation" : {
+          "radians" : 2.5582681034202466
+        }
+      },
+      "curvature" : -0.6027205654193771
+    }, {
+      "time" : 1.2315199026124195,
+      "velocity" : 1.6084681215813614,
+      "acceleration" : -3.2340381044624498,
+      "pose" : {
+        "translation" : {
+          "x" : 2.104181367682031,
+          "y" : 0.6612942646315787
+        },
+        "rotation" : {
+          "radians" : 2.501083096666535
+        }
+      },
+      "curvature" : -0.7854142700643647
+    }, {
+      "time" : 1.2832918933759534,
+      "velocity" : 1.441035530708215,
+      "acceleration" : -2.161144532997304,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0425044814626396,
+          "y" : 0.7105628803291779
+        },
+        "rotation" : {
+          "radians" : 2.431516535185916
+        }
+      },
+      "curvature" : -0.978530406846595
+    }, {
+      "time" : 1.3381225717901073,
+      "velocity" : 1.322538509812933,
+      "acceleration" : -1.509248244590621,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9870436345296412,
+          "y" : 0.7621799957451003
+        },
+        "rotation" : {
+          "radians" : 2.3503134945481206
+        }
+      },
+      "curvature" : -1.161734937054165
+    }, {
+      "time" : 1.3664915633080867,
+      "velocity" : 1.2797226591636164,
+      "acceleration" : -1.1263537161568526,
+      "pose" : {
+        "translation" : {
+          "x" : 1.961679353004223,
+          "y" : 0.7889966218317799
+        },
+        "rotation" : {
+          "radians" : 2.3059373339142457
+        }
+      },
+      "curvature" : -1.24077199916201
+    }, {
+      "time" : 1.3952936681311403,
+      "velocity" : 1.2472813013630308,
+      "acceleration" : -0.7667596602916469,
+      "pose" : {
+        "translation" : {
+          "x" : 1.937899033320381,
+          "y" : 0.8165436519335003
+        },
+        "rotation" : {
+          "radians" : 2.2595451461663565
+        }
+      },
+      "curvature" : -1.306155490343675
+    }, {
+      "time" : 1.424400522054752,
+      "velocity" : 1.2249633399364037,
+      "acceleration" : -0.42132507119357915,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9156993576789647,
+          "y" : 0.8448581059673881
+        },
+        "rotation" : {
+          "radians" : 2.2116259567064747
+        }
+      },
+      "curvature" : -1.3541835082656821
+    }, {
+      "time" : 1.4536761435963022,
+      "velocity" : 1.2126287866061738,
+      "acceleration" : -0.08170141033781976,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8950700272107497,
+          "y" : 0.8739710818044841
+        },
+        "rotation" : {
+          "radians" : 2.162745065781283
+        }
+      },
+      "curvature" : -1.3818724424188715
+    }, {
+      "time" : 1.4829785112196618,
+      "velocity" : 1.210234741845108,
+      "acceleration" : 0.26045790812859687,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8759934232245703,
+          "y" : 0.9039073195012026
+        },
+        "rotation" : {
+          "radians" : 2.113522302547328
+        }
+      },
+      "curvature" : -1.387344994997751
+    }, {
+      "time" : 1.5121614711143974,
+      "velocity" : 1.2178356745322916,
+      "acceleration" : 0.6143648008907316,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8584442684554539,
+          "y" : 0.9346847655307897
+        },
+        "rotation" : {
+          "radians" : 2.064604229359281
+        }
+      },
+      "curvature" : -1.3700812404747664
+    }, {
+      "time" : 1.5410766974782304,
+      "velocity" : 1.2356001718200182,
+      "acceleration" : 0.9912142506014653,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8423892883127544,
+          "y" : 0.9663141370147823
+        },
+        "rotation" : {
+          "radians" : 2.0166336726291316
+        }
+      },
+      "curvature" : -1.3309685196438428
+    }, {
+      "time" : 1.5695753786663202,
+      "velocity" : 1.2638484707370008,
+      "acceleration" : 1.6439611484263257,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8277868721282857,
+          "y" : 0.9987984859544667
+        },
+        "rotation" : {
+          "radians" : 1.9702206136785245
+        }
+      },
+      "curvature" : -1.2721364283154866
+    }, {
+      "time" : 1.6245807356379516,
+      "velocity" : 1.3542751405536837,
+      "acceleration" : 2.798992607878158,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8027295760623936,
+          "y" : 1.0663033839935565
+        },
+        "rotation" : {
+          "radians" : 1.8842065669188885
+        }
+      },
+      "curvature" : -1.1079237968679712
+    }, {
+      "time" : 1.676119381405686,
+      "velocity" : 1.4985314290776237,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 1.782759900790552,
+          "y" : 1.137054014048772
+        },
+        "rotation" : {
+          "radians" : 1.8100759100562795
+        }
+      },
+      "curvature" : -0.904882088448944
+    }, {
+      "time" : 1.7234002817652252,
+      "velocity" : 1.6876550305157805,
+      "acceleration" : 3.9999999999999885,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7672103094854315,
+          "y" : 1.2107543994101775
+        },
+        "rotation" : {
+          "radians" : 1.7501344274699886
+        }
+      },
+      "curvature" : -0.6861541138888728
+    }, {
+      "time" : 1.7668603468362696,
+      "velocity" : 1.8614952907999578,
+      "acceleration" : -2.6118583184812723,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7552473678998695,
+          "y" : 1.2869440876638691
+        },
+        "rotation" : {
+          "radians" : 1.7057998025006624
+        }
+      },
+      "curvature" : -0.4641078115451259
+    }, {
+      "time" : 1.8104168065342312,
+      "velocity" : 1.7477319892142427,
+      "acceleration" : -4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7458609043071398,
+          "y" : 1.3649842060986543
+        },
+        "rotation" : {
+          "radians" : 1.6781427516970726
+        }
+      },
+      "curvature" : -0.23909570910678915
+    }, {
+      "time" : 1.858532964177138,
+      "velocity" : 1.5552673586426153,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7378531694412231,
+          "y" : 1.444043517112732
+        },
+        "rotation" : {
+          "radians" : 1.668497763241333
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 1.9135617600953347,
+      "velocity" : 1.3351521749698287,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 1.731150510518038,
+          "y" : 1.5232887159155766
+        },
+        "rotation" : {
+          "radians" : 1.6290693879713087
+        }
+      },
+      "curvature" : -0.9651764780400796
+    }, {
+      "time" : 1.9447141490181088,
+      "velocity" : 1.2105426192787325,
+      "acceleration" : -3.8758391033227886,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7297123504678038,
+          "y" : 1.5629148639371933
+        },
+        "rotation" : {
+          "radians" : 1.582290663183503
+        }
+      },
+      "curvature" : -1.3866393975350608
+    }, {
+      "time" : 1.9793752955290544,
+      "velocity" : 1.0762015922656092,
+      "acceleration" : -2.0744901253079546,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7304462942776127,
+          "y" : 1.6025386552453256
+        },
+        "rotation" : {
+          "radians" : 1.5198280388303091
+        }
+      },
+      "curvature" : -1.7544316082551803
+    }, {
+      "time" : 2.0177456093542387,
+      "velocity" : 0.9966027551302967,
+      "acceleration" : -1.0463551078296154,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7339390283538525,
+          "y" : 1.6421520518968291
+        },
+        "rotation" : {
+          "radians" : 1.4439430669964353
+        }
+      },
+      "curvature" : -2.0458770787946707
+    }, {
+      "time" : 2.0589400093906947,
+      "velocity" : 0.9534987842381746,
+      "acceleration" : -0.4880905611008773,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7407113486278938,
+          "y" : 1.6817436414219813
+        },
+        "rotation" : {
+          "radians" : 1.3575512036532698
+        }
+      },
+      "curvature" : -2.2350303059988965
+    }, {
+      "time" : 2.0803982879163243,
+      "velocity" : 0.9430252010323411,
+      "acceleration" : -0.17603090665285,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7454726503711389,
+          "y" : 1.70152681499276
+        },
+        "rotation" : {
+          "radians" : 1.3115071881305223
+        }
+      },
+      "curvature" : -2.2849521300818747
+    }, {
+      "time" : 2.1022775055735607,
+      "velocity" : 0.9391737825112827,
+      "acceleration" : 0.10804204147343682,
+      "pose" : {
+        "translation" : {
+          "x" : 1.751220456348885,
+          "y" : 1.7212988221176222
+        },
+        "rotation" : {
+          "radians" : 1.2642073316351676
+        }
+      },
+      "curvature" : -2.3037310856346584
+    }, {
+      "time" : 2.124492132931374,
+      "velocity" : 0.9415738962015925,
+      "acceleration" : 0.3726592944849579,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7580023567016838,
+          "y" : 1.7410573696656044
+        },
+        "rotation" : {
+          "radians" : 1.2161486950907356
+        }
+      },
+      "curvature" : -2.2920014286625774
+    }, {
+      "time" : 2.146960488972026,
+      "velocity" : 0.9499469379119386,
+      "acceleration" : 0.6237338591278402,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7658622538765485,
+          "y" : 1.7607999883402954
+        },
+        "rotation" : {
+          "radians" : 1.1678204422268519
+        }
+      },
+      "curvature" : -2.2517750837082873
+    }, {
+      "time" : 2.1696053252037935,
+      "velocity" : 0.9640712890040968,
+      "acceleration" : 0.8654335988347971,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7748404343704771,
+          "y" : 1.780524038470248
+        },
+        "rotation" : {
+          "radians" : 1.119686904181982
+        }
+      },
+      "curvature" : -2.1862781024111237
+    }, {
+      "time" : 2.192354484026944,
+      "velocity" : 0.9837591753948806,
+      "acceleration" : 1.1007322784603935,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7849736404739778,
+          "y" : 1.8002267157993888
+        },
+        "rotation" : {
+          "radians" : 1.0721723951266355
+        }
+      },
+      "curvature" : -2.0996461580977543
+    }, {
+      "time" : 2.215141507868605,
+      "velocity" : 1.008841588067443,
+      "acceleration" : 1.3317674781680595,
+      "pose" : {
+        "translation" : {
+          "x" : 1.796295142014595,
+          "y" : 1.819905057277429
+        },
+        "rotation" : {
+          "radians" : 1.025649276755208
+        }
+      },
+      "curvature" : -1.996538776374713
+    }, {
+      "time" : 2.2379061099913464,
+      "velocity" : 1.0391587448279456,
+      "acceleration" : 1.6754933878837617,
+      "pose" : {
+        "translation" : {
+          "x" : 1.808834808100432,
+          "y" : 1.8395559468502753
+        },
+        "rotation" : {
+          "radians" : 0.9804301798041166
+        }
+      },
+      "curvature" : -1.8817412716281585
+    }, {
+      "time" : 2.2830950022778635,
+      "velocity" : 1.1148724350597967,
+      "acceleration" : 2.127602917951782,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8376715372041317,
+          "y" : 1.8787621757874542
+        },
+        "rotation" : {
+          "radians" : 0.8948396405471469
+        }
+      },
+      "curvature" : -1.6348328210818963
+    }, {
+      "time" : 2.3276395725958383,
+      "velocity" : 1.2096455928472285,
+      "acceleration" : 2.5789601207010184,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8716574925150553,
+          "y" : 1.9178176341376911
+        },
+        "rotation" : {
+          "radians" : 0.8166729724145454
+        }
+      },
+      "curvature" : -1.3886967164626418
+    }, {
+      "time" : 2.371283333319735,
+      "velocity" : 1.3222011112715757,
+      "acceleration" : 3.0313394226052046,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9109165188377353,
+          "y" : 1.9566924759531596
+        },
+        "rotation" : {
+          "radians" : 0.7463747869451778
+        }
+      },
+      "curvature" : -1.1623279144475585
+    }, {
+      "time" : 2.4138523222192685,
+      "velocity" : 1.4512421655031749,
+      "acceleration" : 3.4847988479105942,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9555249368440535,
+          "y" : 1.9953549631045817
+        },
+        "rotation" : {
+          "radians" : 0.683796860013437
+        }
+      },
+      "curvature" : -0.9648147341420715
+    }, {
+      "time" : 2.455238313142531,
+      "velocity" : 1.5954640189921991,
+      "acceleration" : 3.93825151072416,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0055138388660367,
+          "y" : 2.0337716505743684
+        },
+        "rotation" : {
+          "radians" : 0.6284298787308873
+        }
+      },
+      "curvature" : -0.7982697547383403
+    }, {
+      "time" : 2.49538256040239,
+      "velocity" : 1.7535621614102237,
+      "acceleration" : 4.000000000000021,
+      "pose" : {
+        "translation" : {
+          "x" : 2.060871384688653,
+          "y" : 2.0719075717497617
+        },
+        "rotation" : {
+          "radians" : 0.5795857801198268
+        }
+      },
+      "curvature" : -0.6608172515589942
+    }, {
+      "time" : 2.53441622407071,
+      "velocity" : 1.9096968160835055,
+      "acceleration" : 4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.121545097342608,
+          "y" : 2.1097264237159736
+        },
+        "rotation" : {
+          "radians" : 0.5365211642557431
+        }
+      },
+      "curvature" : -0.5487920239178083
+    }, {
+      "time" : 2.5725848078202302,
+      "velocity" : 2.062371151081586,
+      "acceleration" : 4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 2.18744415889714,
+          "y" : 2.147190752549328
+        },
+        "rotation" : {
+          "radians" : 0.4985109954494367
+        }
+      },
+      "curvature" : -0.45807197306180775
+    }, {
+      "time" : 2.6100585451602245,
+      "velocity" : 2.212266100441563,
+      "acceleration" : 3.9999999999999893,
+      "pose" : {
+        "translation" : {
+          "x" : 2.258441706252816,
+          "y" : 2.1842621386104017
+        },
+        "rotation" : {
+          "radians" : 0.4648869468393921
+        }
+      },
+      "curvature" : -0.3847621795776758
+    }, {
+      "time" : 2.646940241630883,
+      "velocity" : 2.359792886324197,
+      "acceleration" : 4.00000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 2.334377126934328,
+          "y" : 2.220901381837166
+        },
+        "rotation" : {
+          "radians" : 0.43505326862724697
+        }
+      },
+      "curvature" : -0.32547741802200536
+    }, {
+      "time" : 2.683288527639372,
+      "velocity" : 2.505186030358152,
+      "acceleration" : 4.000000000000014,
+      "pose" : {
+        "translation" : {
+          "x" : 2.415058354883288,
+          "y" : 2.2570686870381245
+        },
+        "rotation" : {
+          "radians" : 0.4084896240286113
+        }
+      },
+      "curvature" : -0.2774076507783822
+    }, {
+      "time" : 2.719132405668665,
+      "velocity" : 2.6485615424753246,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.500264166251025,
+          "y" : 2.2927238491854567
+        },
+        "rotation" : {
+          "radians" : 0.384747078396271
+        }
+      },
+      "curvature" : -0.23828130083939184
+    }, {
+      "time" : 2.754480704601807,
+      "velocity" : 2.789954738207894,
+      "acceleration" : 2.9488324318429173,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5897464751913803,
+          "y" : 2.3278264387081586
+        },
+        "rotation" : {
+          "radians" : 0.3634409880883115
+        }
+      },
+      "curvature" : -0.20628975468482194
+    }, {
+      "time" : 2.7895490251950776,
+      "velocity" : 2.8933653393035956,
+      "acceleration" : 0.7037966720327286,
+      "pose" : {
+        "translation" : {
+          "x" : 2.683232629653503,
+          "y" : 2.362335986785183
+        },
+        "rotation" : {
+          "radians" : 0.34424291191521855
+        }
+      },
+      "curvature" : -0.18000474642406875
+    }, {
+      "time" : 2.8249707390558063,
+      "velocity" : 2.918295023636472,
+      "acceleration" : 0.5763378030245723,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7804277071746464,
+          "y" : 2.39621217063858
+        },
+        "rotation" : {
+          "radians" : 0.32687266278130106
+        }
+      },
+      "curvature" : -0.15830253678418688
+    }, {
+      "time" : 2.861139234917162,
+      "velocity" : 2.939140295079909,
+      "acceleration" : 0.4740600472954994,
+      "pose" : {
+        "translation" : {
+          "x" : 2.881016810672964,
+          "y" : 2.429414998826637
+        },
+        "rotation" : {
+          "radians" : 0.31109102748658707
+        }
+      },
+      "curvature" : -0.1402996898246936
+    }, {
+      "time" : 2.8979872681123133,
+      "velocity" : 2.9566084754391486,
+      "acceleration" : 0.3917399621578322,
+      "pose" : {
+        "translation" : {
+          "x" : 2.984667364240305,
+          "y" : 2.4619049965370223
+        },
+        "rotation" : {
+          "radians" : 0.2966933532809512
+        }
+      },
+      "curvature" : -0.1253009626312453
+    }, {
+      "time" : 2.935436818258982,
+      "velocity" : 2.9712789607964325,
+      "acceleration" : 0.3251659903474371,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0910314089350095,
+          "y" : 2.4936433908799245
+        },
+        "rotation" : {
+          "radians" : 0.28350402582744755
+        }
+      },
+      "curvature" : -0.11275805306566417
+    }, {
+      "time" : 2.9734007910904325,
+      "velocity" : 2.9836235536196942,
+      "acceleration" : 0.2709852112638229,
+      "pose" : {
+        "translation" : {
+          "x" : 3.199747898574706,
+          "y" : 2.5245922961811917
+        },
+        "rotation" : {
+          "radians" : 0.2713717786654788
+        }
+      },
+      "curvature" : -0.10223741084606355
+    }, {
+      "time" : 3.0117845646161134,
+      "velocity" : 2.9940249885976535,
+      "acceleration" : 0.1543098408303897,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3104449955291066,
+          "y" : 2.5547148992754742
+        },
+        "rotation" : {
+          "radians" : 0.2601657392085518
+        }
+      },
+      "curvature" : -0.09339533006984713
+    }, {
+      "time" : 3.0505054353667576,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 7.591176208555741E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.422742366512802,
+          "y" : 2.5839756447993656
+        },
+        "rotation" : {
+          "radians" : 0.24977210747664394
+        }
+      },
+      "curvature" : -0.08595876803031433
+    }, {
+      "time" : 3.0895059097464324,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5362534783780575,
+          "y" : 2.612340420484543
+        },
+        "rotation" : {
+          "radians" : 0.24009136790995844
+        }
+      },
+      "curvature" : -0.07971061642849785
+    }, {
+      "time" : 3.1286993195347663,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.650587893907611,
+          "y" : 2.6397767424509064
+        },
+        "rotation" : {
+          "radians" : 0.23103594442528902
+        }
+      },
+      "curvature" : -0.07447841843016513
+    }, {
+      "time" : 3.16795942350856,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.765353567607466,
+          "y" : 2.666253940499722
+        },
+        "rotation" : {
+          "radians" : 0.22252822046257315
+        }
+      },
+      "curvature" : -0.07012575415648367
+    }, {
+      "time" : 3.2071598053906003,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8801591414996897,
+          "y" : 2.6917433434067615
+        },
+        "rotation" : {
+          "radians" : 0.21449885716390527
+        }
+      },
+      "curvature" : -0.06654570394142899
+    }, {
+      "time" : 3.2461747016130165,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9946162409152066,
+          "y" : 2.7162184642154443
+        },
+        "rotation" : {
+          "radians" : 0.20688535313560402
+        }
+      },
+      "curvature" : -0.06365594712905223
+    }, {
+      "time" : 3.2848798203338982,
+      "velocity" : 3.0,
+      "acceleration" : -7.735398279518994E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.1083417702865965,
+          "y" : 2.7396551855299762
+        },
+        "rotation" : {
+          "radians" : 0.19963079814758483
+        }
+      },
+      "curvature" : -0.06139517108104972
+    }, {
+      "time" : 3.32315315320381,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 7.84832578379943E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.220960208940891,
+          "y" : 2.7620319448084913
+        },
+        "rotation" : {
+          "radians" : 0.19268278060685093
+        }
+      },
+      "curvature" : -0.05972055737308964
+    }, {
+      "time" : 3.3608757811105803,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.332105906892365,
+          "y" : 2.783329919656194
+        },
+        "rotation" : {
+          "radians" : 0.18599241484195372
+        }
+      },
+      "curvature" : -0.0586061860352566
+    }, {
+      "time" : 3.3979326749154137,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.441425380635337,
+          "y" : 2.8035332131184987
+        },
+        "rotation" : {
+          "radians" : 0.17951345937534083
+        }
+      },
+      "curvature" : -0.05804225916920248
+    }, {
+      "time" : 3.43421349202006,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.548579608936964,
+          "y" : 2.8226290389741697
+        },
+        "rotation" : {
+          "radians" : 0.17320150172377563
+        }
+      },
+      "curvature" : -0.058035096135246704
+    }, {
+      "time" : 3.4696133694646405,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.653246328630036,
+          "y" : 2.8406079070284633
+        },
+        "rotation" : {
+          "radians" : 0.16701318920823915
+        }
+      },
+      "curvature" : -0.05860789609869524
+    }, {
+      "time" : 3.5040337141415066,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.755122330405774,
+          "y" : 2.857463808406269
+        },
+        "rotation" : {
+          "radians" : 0.16090548924261228
+        }
+      },
+      "curvature" : -0.05980230049917739
+    }, {
+      "time" : 3.5373829906159067,
+      "velocity" : 3.0,
+      "acceleration" : -1.3000955620508416,
+      "pose" : {
+        "translation" : {
+          "x" : 4.853925754606621,
+          "y" : 2.87319440084525
+        },
+        "rotation" : {
+          "radians" : 0.15483496726687765
+        }
+      },
+      "curvature" : -0.06168081541885887
+    }, {
+      "time" : 3.5698052848244366,
+      "velocity" : 2.9578479191879836,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.9493983870190466,
+          "y" : 2.8878011939889827
+        },
+        "rotation" : {
+          "radians" : 0.14875707688110845
+        }
+      },
+      "curvature" : -0.0643301641863426
+    }, {
+      "time" : 3.601908104387069,
+      "velocity" : 2.8294366409374536,
+      "acceleration" : -3.999999999999995,
+      "pose" : {
+        "translation" : {
+          "x" : 5.041307954666333,
+          "y" : 2.9012897346800983
+        },
+        "rotation" : {
+          "radians" : 0.1426254663359473
+        }
+      },
+      "curvature" : -0.06786561693146706
+    }, {
+      "time" : 3.634098277881151,
+      "velocity" : 2.700675946961127,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.12945042160138,
+          "y" : 2.913669792253425
+        },
+        "rotation" : {
+          "radians" : 0.13639132070467594
+        }
+      },
+      "curvature" : -0.07243625256641337
+    }, {
+      "time" : 3.6663242273354184,
+      "velocity" : 2.5717721491440564,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.213652284699492,
+          "y" : 2.924955543829128
+        },
+        "rotation" : {
+          "radians" : 0.13000278348350505
+        }
+      },
+      "curvature" : -0.07823088630580577
+    }, {
+      "time" : 3.6985369799811316,
+      "velocity" : 2.4429211385612026,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 5.293772869451182,
+          "y" : 2.935165759605849
+        },
+        "rotation" : {
+          "radians" : 0.12340454072534943
+        }
+      },
+      "curvature" : -0.08548392661118162
+    }, {
+      "time" : 3.730691893404645,
+      "velocity" : 2.314301484867149,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.3697066257549615,
+          "y" : 2.9443239881538474
+        },
+        "rotation" : {
+          "radians" : 0.11653771369919076
+        }
+      },
+      "curvature" : -0.09447950544669352
+    }, {
+      "time" : 3.7627510266183024,
+      "velocity" : 2.1860649520125195,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.441385423710139,
+          "y" : 2.952458741708144
+        },
+        "rotation" : {
+          "radians" : 0.10934030498899684
+        }
+      },
+      "curvature" : -0.10555051064633172
+    }, {
+      "time" : 3.7946864190689498,
+      "velocity" : 2.058323382209931,
+      "acceleration" : -3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 5.508780849409618,
+          "y" : 2.959603681461658
+        },
+        "rotation" : {
+          "radians" : 0.10174859495289124
+        }
+      },
+      "curvature" : -0.1190660934873409
+    }, {
+      "time" : 3.8581528018765208,
+      "velocity" : 1.8044578509796478,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.630820283137819,
+          "y" : 2.971085620886363
+        },
+        "rotation" : {
+          "radians" : 0.08513910857652777
+        }
+      },
+      "curvature" : -0.15483239704424728
+    }, {
+      "time" : 3.9212966580627038,
+      "velocity" : 1.5518824262349158,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.73647917568087,
+          "y" : 2.9791491162686725
+        },
+        "rotation" : {
+          "radians" : 0.0663518340621249
+        }
+      },
+      "curvature" : -0.20277252875109172
+    }, {
+      "time" : 3.985088994027884,
+      "velocity" : 1.296713082374195,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.827194162416493,
+          "y" : 2.98426771953671
+        },
+        "rotation" : {
+          "radians" : 0.045585623619075005
+        }
+      },
+      "curvature" : -0.2546275827711898
+    }, {
+      "time" : 4.0522933348683985,
+      "velocity" : 1.027895719012139,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.9052577307282945,
+          "y" : 2.987015154086521
+        },
+        "rotation" : {
+          "radians" : 0.02445962652264226
+        }
+      },
+      "curvature" : -0.27725843604659994
+    }, {
+      "time" : 4.131181440085891,
+      "velocity" : 0.7123432981421679,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.973891685375232,
+          "y" : 2.988071244162576
+        },
+        "rotation" : {
+          "radians" : 0.007123366020833881
+        }
+      },
+      "curvature" : -0.20476533171945074
+    }, {
+      "time" : 4.309267264621433,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.037320613861084,
+          "y" : 2.9882278442382812
+        },
+        "rotation" : {
+          "radians" : 0.0
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 4.056426048278809, -3.3782451152801514, 0.0 ],
+      "y" : [ -0.5667314529418945, 2.785569906234741, 0.0 ]
+    }, {
+      "x" : [ 1.7378531694412231, -0.24853384494781494, 0.0 ],
+      "y" : [ 1.444043517112732, 2.535710334777832, 0.0 ]
+    }, {
+      "x" : [ 6.037320613861084, 2.0, 0.0 ],
+      "y" : [ 2.9882278442382812, 0.0, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.2873733127254927
+      }
+    }, {
+      "time" : 1.8499999586492777,
+      "rotation" : {
+        "radians" : 2.3228735180023055
+      }
+    }, {
+      "time" : 4.21999990567565,
+      "rotation" : {
+        "radians" : 0.6128208180837543
+      }
+    } ],
+    "reversed" : false,
+    "color" : 124.23529,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  } ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/autos/red/hidecargo.json b/src/main/deploy/autos/red/hidecargo.json
new file mode 100644
index 00000000..16cbc83b
--- /dev/null
+++ b/src/main/deploy/autos/red/hidecargo.json
@@ -0,0 +1,3175 @@
+{
+  "autonomousSteps" : [ {
+    "type" : "script",
+    "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "OPEN" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "INTAKE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "ON" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.1293768882751465,
+          "y" : 1.0937262773513794
+        },
+        "rotation" : {
+          "radians" : 2.4178655244299407
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.24452127463632814,
+      "velocity" : 0.9780850985453124,
+      "acceleration" : 3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.039938468317359,
+          "y" : 1.173101702633744
+        },
+        "rotation" : {
+          "radians" : 2.411607121974174
+        }
+      },
+      "curvature" : -0.10063347960741297
+    }, {
+      "time" : 0.3430023265109939,
+      "velocity" : 1.3720093060439753,
+      "acceleration" : 3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.954285601692391,
+          "y" : 1.2509135147447523
+        },
+        "rotation" : {
+          "radians" : 2.395009715228919
+        }
+      },
+      "curvature" : -0.1855623376134662
+    }, {
+      "time" : 0.4150553084726476,
+      "velocity" : 1.6602212338905902,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.87496604004582,
+          "y" : 1.3260262403808838
+        },
+        "rotation" : {
+          "radians" : 2.3703948601089895
+        }
+      },
+      "curvature" : -0.26531768438500725
+    }, {
+      "time" : 0.4721670734641104,
+      "velocity" : 1.8886682938564414,
+      "acceleration" : 3.999999999999993,
+      "pose" : {
+        "translation" : {
+          "x" : 5.803342186380178,
+          "y" : 1.3977211365709081
+        },
+        "rotation" : {
+          "radians" : 2.339767645326564
+        }
+      },
+      "curvature" : -0.3381985496587611
+    }, {
+      "time" : 0.5190911939858679,
+      "velocity" : 2.076364775943471,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.7397725735722815,
+          "y" : 1.4656408262000014
+        },
+        "rotation" : {
+          "radians" : 2.3056558386592805
+        }
+      },
+      "curvature" : -0.3907488038191573
+    }, {
+      "time" : 0.5585735987097502,
+      "velocity" : 2.2342943948390004,
+      "acceleration" : 0.26324528204804704,
+      "pose" : {
+        "translation" : {
+          "x" : 5.683793342890567,
+          "y" : 1.529733933533862
+        },
+        "rotation" : {
+          "radians" : 2.271678768601324
+        }
+      },
+      "curvature" : -0.39766870774643054
+    }, {
+      "time" : 0.5934744799766615,
+      "velocity" : 2.243481887171834,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.634299722512424,
+          "y" : 1.5901997197428273
+        },
+        "rotation" : {
+          "radians" : 2.242697343821613
+        }
+      },
+      "curvature" : -0.32812227024366547
+    }, {
+      "time" : 0.6267989018195489,
+      "velocity" : 2.110184199800284,
+      "acceleration" : -3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 5.589727506041527,
+          "y" : 1.6474327184259892
+        },
+        "rotation" : {
+          "radians" : 2.224227521884603
+        }
+      },
+      "curvature" : -0.16341344710143665
+    }, {
+      "time" : 0.6603386127840065,
+      "velocity" : 1.9760253559424537,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.548234531025173,
+          "y" : 1.7019673711353107
+        },
+        "rotation" : {
+          "radians" : 2.2209763317101947
+        }
+      },
+      "curvature" : 0.07956164338822755
+    }, {
+      "time" : 0.6950498794552016,
+      "velocity" : 1.8371802892576732,
+      "acceleration" : -4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 5.507882157471613,
+          "y" : 1.7544226628997421
+        },
+        "rotation" : {
+          "radians" : 2.234944043969041
+        }
+      },
+      "curvature" : 0.33953937734428047
+    }, {
+      "time" : 0.732203292039014,
+      "velocity" : 1.6885666389224232,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.466816746367385,
+          "y" : 1.805446757749337
+        },
+        "rotation" : {
+          "radians" : 2.264150133653273
+        }
+      },
+      "curvature" : 0.5355105351904276
+    }, {
+      "time" : 0.773517717518194,
+      "velocity" : 1.5233089370057031,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.42345113819465,
+          "y" : 1.8556616342393681
+        },
+        "rotation" : {
+          "radians" : 2.30296870864004
+        }
+      },
+      "curvature" : 0.6126486386243994
+    }, {
+      "time" : 0.8214715782483804,
+      "velocity" : 1.3314934940849577,
+      "acceleration" : -3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 5.376646131448524,
+          "y" : 1.9056077209744444
+        },
+        "rotation" : {
+          "radians" : 2.343996330815072
+        }
+      },
+      "curvature" : 0.568293999561127
+    }, {
+      "time" : 0.8802038597478065,
+      "velocity" : 1.0965643680872534,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.3258919611544115,
+          "y" : 1.9556885321326263
+        },
+        "rotation" : {
+          "radians" : 2.3801609988469874
+        }
+      },
+      "curvature" : 0.4354667748411578
+    }, {
+      "time" : 0.9592448110914289,
+      "velocity" : 0.7804005627127639,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.271489777385341,
+          "y" : 2.006115302989542
+        },
+        "rotation" : {
+          "radians" : 2.405692727133013
+        }
+      },
+      "curvature" : 0.24590943352090008
+    }, {
+      "time" : 1.1543449517696198,
+      "velocity" : 0.0,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.214733123779297,
+          "y" : 2.056851625442505
+        },
+        "rotation" : {
+          "radians" : 2.4154850346736763
+        }
+      },
+      "curvature" : -0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 6.1293768882751465, -1.4423890113830566, 0.0 ],
+      "y" : [ 1.0937262773513794, 1.27461576461792, 0.0 ]
+    }, {
+      "x" : [ 5.214733123779297, -0.9154930114746094, 0.0 ],
+      "y" : [ 2.056851625442505, 0.812895655632019, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.37094272169677
+      }
+    }, {
+      "time" : 1.0699999760836363,
+      "rotation" : {
+        "radians" : 2.370942522371697
+      }
+    } ],
+    "reversed" : false,
+    "color" : 209.88235,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 7",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.VisionManager.shootBalls",
+        "args" : [ "7" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "Hopper.setEjectOverride true\nDrive.turnToAngle 45",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setEjectOverride",
+        "args" : [ "true" ],
+        "argTypes" : [ "boolean" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Drive.turnToAngle",
+        "args" : [ "45" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.214733123779297,
+          "y" : 2.056851625442505
+        },
+        "rotation" : {
+          "radians" : 0.9530775920796433
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.22164892589636578,
+      "velocity" : 0.8865957035854634,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.272283410420641,
+          "y" : 2.136490201934066
+        },
+        "rotation" : {
+          "radians" : 0.93154333853559
+        }
+      },
+      "curvature" : -0.3354703724748357
+    }, {
+      "time" : 0.32212126730993046,
+      "velocity" : 1.2884850692397223,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 5.339010516181588,
+          "y" : 2.223017249489203
+        },
+        "rotation" : {
+          "radians" : 0.8980578131179813
+        }
+      },
+      "curvature" : -0.24297628723974596
+    }, {
+      "time" : 0.40798632695507736,
+      "velocity" : 1.6319453078203097,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.418256742414087,
+          "y" : 2.320179727881623
+        },
+        "rotation" : {
+          "radians" : 0.8787668506583872
+        }
+      },
+      "curvature" : -0.07195280186555318
+    }, {
+      "time" : 0.4482103474158235,
+      "velocity" : 1.7928413896632942,
+      "acceleration" : 4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 5.462294308261335,
+          "y" : 2.373142516471262
+        },
+        "rotation" : {
+          "radians" : 0.8763509051285007
+        }
+      },
+      "curvature" : -0.0010310805010032027
+    }, {
+      "time" : 0.48701783511066765,
+      "velocity" : 1.9480713404426708,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.508707731962204,
+          "y" : 2.428952641785145
+        },
+        "rotation" : {
+          "radians" : 0.8784798678297706
+        }
+      },
+      "curvature" : 0.057187573384384974
+    }, {
+      "time" : 0.5244749433253648,
+      "velocity" : 2.0978997733014593,
+      "acceleration" : -3.006834100609329,
+      "pose" : {
+        "translation" : {
+          "x" : 5.5569134053257585,
+          "y" : 2.4874170951864016
+        },
+        "rotation" : {
+          "radians" : 0.8846611837531144
+        }
+      },
+      "curvature" : 0.10384088533571874
+    }, {
+      "time" : 0.5628765788352517,
+      "velocity" : 1.9824324261311614,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 5.606271566124633,
+          "y" : 2.5482597037116648
+        },
+        "rotation" : {
+          "radians" : 0.8942836471940969
+        }
+      },
+      "curvature" : 0.13988861038856043
+    }, {
+      "time" : 0.6051667706790917,
+      "velocity" : 1.813271658755801,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.656145019875112,
+          "y" : 2.6111436507878807
+        },
+        "rotation" : {
+          "radians" : 0.9066044762714179
+        }
+      },
+      "curvature" : 0.16515378657020438
+    }, {
+      "time" : 0.6526161179857626,
+      "velocity" : 1.6234742695291178,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.7059578616172075,
+          "y" : 2.6756939969491214
+        },
+        "rotation" : {
+          "radians" : 0.9206836126942823
+        }
+      },
+      "curvature" : 0.17781422207911715
+    }, {
+      "time" : 0.7069026874415512,
+      "velocity" : 1.4063279917059635,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.75525419769474,
+          "y" : 2.7415202005533956
+        },
+        "rotation" : {
+          "radians" : 0.9352834691742239
+        }
+      },
+      "curvature" : 0.1740904256082478
+    }, {
+      "time" : 0.7714879126896856,
+      "velocity" : 1.1479870907134255,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.803756867535412,
+          "y" : 2.8082386384994606
+        },
+        "rotation" : {
+          "radians" : 0.9487507880978892
+        }
+      },
+      "curvature" : 0.14816445891139096
+    }, {
+      "time" : 0.8556329295191124,
+      "velocity" : 0.8114070233957184,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.851426165430894,
+          "y" : 2.8754951269436333
+        },
+        "rotation" : {
+          "radians" : 0.9589069733276255
+        }
+      },
+      "curvature" : 0.09262519823750537
+    }, {
+      "time" : 1.058484685368042,
+      "velocity" : 0.0,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.8985185623168945,
+          "y" : 2.9429874420166016
+        },
+        "rotation" : {
+          "radians" : 0.9629949901604784
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.214733123779297, 0.4449276924133301, 0.0 ],
+      "y" : [ 2.056851625442505, 0.6262435913085938, 0.0 ]
+    }, {
+      "x" : [ 5.8985185623168945, 0.7516036033630371, 0.0 ],
+      "y" : [ 2.9429874420166016, 1.0804316997528076, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.9599310885968813
+      }
+    }, {
+      "time" : 0.9699999783188105,
+      "rotation" : {
+        "radians" : 0.9599310885968813
+      }
+    } ],
+    "reversed" : false,
+    "color" : 148.47061,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@t 1.3\nIntake.setIntakeSolState CLOSE\nIntake.setWantedIntakeState OFF\nHopper.setHopperState OFF",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 1.3,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "CLOSE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "OFF" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "OFF" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "@t 3.36\nIntake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 3.36,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "OPEN" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "INTAKE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "ON" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.8985185623168945,
+          "y" : 2.9429874420166016
+        },
+        "rotation" : {
+          "radians" : -3.1024027203970066
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.2301838401073264,
+      "velocity" : 0.9207353604293056,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.792630460413637,
+          "y" : 2.9388424054094813
+        },
+        "rotation" : {
+          "radians" : -3.1025922709616696
+        }
+      },
+      "curvature" : -0.0034322907229005963
+    }, {
+      "time" : 0.3255946694282438,
+      "velocity" : 1.302378677712975,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.686655575860993,
+          "y" : 2.9347317908029744
+        },
+        "rotation" : {
+          "radians" : -3.1031002310232063
+        }
+      },
+      "curvature" : -0.006005316476928024
+    }, {
+      "time" : 0.3988940171844041,
+      "velocity" : 1.5955760687376164,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.580523715266813,
+          "y" : 2.930682060104536
+        },
+        "rotation" : {
+          "radians" : -3.103837861955969
+        }
+      },
+      "curvature" : -0.007749412153373173
+    }, {
+      "time" : 0.4607866040428335,
+      "velocity" : 1.843146416171334,
+      "acceleration" : 3.999999999999994,
+      "pose" : {
+        "translation" : {
+          "x" : 5.4741821068677154,
+          "y" : 2.926711325853944
+        },
+        "rotation" : {
+          "radians" : -3.104720238709075
+        }
+      },
+      "curvature" : -0.008706622259397081
+    }, {
+      "time" : 0.5154157029130185,
+      "velocity" : 2.0616628116520737,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.367594657212031,
+          "y" : 2.9228297190306436
+        },
+        "rotation" : {
+          "radians" : -3.1056669054798047
+        }
+      },
+      "curvature" : -0.00892606605401968
+    }, {
+      "time" : 0.5649014668690089,
+      "velocity" : 2.259605867476035,
+      "acceleration" : 4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 5.260741207842759,
+          "y" : 2.9190397568610926
+        },
+        "rotation" : {
+          "radians" : -3.106602230083352
+        }
+      },
+      "curvature" : -0.008459920513905771
+    }, {
+      "time" : 0.6104980484072738,
+      "velocity" : 2.441992193629095,
+      "acceleration" : 3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 5.153616791980521,
+          "y" : 2.9153367106261046
+        },
+        "rotation" : {
+          "radians" : -3.1074555011063474
+        }
+      },
+      "curvature" : -0.007360079879286901
+    }, {
+      "time" : 0.6530171873993678,
+      "velocity" : 2.6120687495974706,
+      "acceleration" : 3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 5.0462308912065055,
+          "y" : 2.911708973468194
+        },
+        "rotation" : {
+          "radians" : -3.108160812688321
+        }
+      },
+      "curvature" : -0.005675482209767876
+    }, {
+      "time" : 0.6930174258760853,
+      "velocity" : 2.7720697035043407,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.93860669214543,
+          "y" : 2.9081384281989218
+        },
+        "rotation" : {
+          "radians" : -3.108656779571012
+        }
+      },
+      "curvature" : -0.003450046167250236
+    }, {
+      "time" : 0.7309003592448649,
+      "velocity" : 2.923601436979459,
+      "acceleration" : 2.0944239970255154,
+      "pose" : {
+        "translation" : {
+          "x" : 4.830780343148486,
+          "y" : 2.904600815106239
+        },
+        "rotation" : {
+          "radians" : -3.108886120874728
+        }
+      },
+      "curvature" : -7.211325069651598E-4
+    }, {
+      "time" : 0.7673774828497517,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.722800210976294,
+          "y" : 2.9010660997618305
+        },
+        "rotation" : {
+          "radians" : -3.10879514573791
+        }
+      },
+      "curvature" : 0.0024815692229093363
+    }, {
+      "time" : 0.8034217930772413,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.614726137481853,
+          "y" : 2.8974988408284617
+        },
+        "rotation" : {
+          "radians" : -3.1083331681482385
+        }
+      },
+      "curvature" : 0.0061368314589584135
+    }, {
+      "time" : 0.839474699342444,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.506628696293496,
+          "y" : 2.8938585578673215
+        },
+        "rotation" : {
+          "radians" : -3.107451872483656
+        }
+      },
+      "curvature" : 0.010232455610621435
+    }, {
+      "time" : 0.8755098996327016,
+      "velocity" : 3.0,
+      "acceleration" : -8.22656662136612E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.39858844949784,
+          "y" : 2.890100099145368
+        },
+        "rotation" : {
+          "radians" : -3.1061046457957544
+        }
+      },
+      "curvature" : 0.01476570595533646
+    }, {
+      "time" : 0.9114981176657808,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.2906952043227395,
+          "y" : 2.886174009442671
+        },
+        "rotation" : {
+          "radians" : -3.1042458879032835
+        }
+      },
+      "curvature" : 0.019743691371007215
+    }, {
+      "time" : 0.9474073803784949,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.270759764362102E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.183047269820236,
+          "y" : 2.8820268978597596
+        },
+        "rotation" : {
+          "radians" : -3.1018303060188313
+        }
+      },
+      "curvature" : 0.025183766010214763
+    }, {
+      "time" : 0.9832033025994578,
+      "velocity" : 3.0,
+      "acceleration" : -8.305527617360462E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.075750713549512,
+          "y" : 2.8776018056249644
+        },
+        "rotation" : {
+          "radians" : -3.0988121969366995
+        }
+      },
+      "curvature" : 0.031114011207342505
+    }, {
+      "time" : 1.0188493791490731,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.349604098090194E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9689186182598455,
+          "y" : 2.8728385739017632
+        },
+        "rotation" : {
+          "radians" : -3.095144716752075
+        }
+      },
+      "curvature" : 0.03757385368866147
+    }, {
+      "time" : 1.0543072846683412,
+      "velocity" : 3.0,
+      "acceleration" : -8.403642908942724E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8626703385735555,
+          "y" : 2.867674211596125
+        },
+        "rotation" : {
+          "radians" : -3.0907791356286958
+        }
+      },
+      "curvature" : 0.0446148697110529
+    }, {
+      "time" : 1.0895371815644033,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7571307576689605,
+          "y" : 2.862043263163855
+        },
+        "rotation" : {
+          "radians" : -3.0856640732526293
+        }
+      },
+      "curvature" : 0.05230182094645797
+    }, {
+      "time" : 1.124498036572409,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.544307135767894E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6524295439633274,
+          "y" : 2.855878176417939
+        },
+        "rotation" : {
+          "radians" : -3.0797447092896437
+        }
+      },
+      "curvature" : 0.06071396531738544
+    }, {
+      "time" : 1.159147946568507,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.548700407795824,
+          "y" : 2.8491096703358885
+        },
+        "rotation" : {
+          "radians" : -3.0729619624239923
+        }
+      },
+      "curvature" : 0.0699466838117423
+    }, {
+      "time" : 1.1934444744263788,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4460803581104718,
+          "y" : 2.841667102867084
+        },
+        "rotation" : {
+          "radians" : -3.0652516314712313
+        }
+      },
+      "curvature" : 0.08011346138708186
+    }, {
+      "time" : 1.2273449958888158,
+      "velocity" : 3.0,
+      "acceleration" : -0.16623210994932952,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3447089591390977,
+          "y" : 2.8334788387401204
+        },
+        "rotation" : {
+          "radians" : -3.0565434927756256
+        }
+      },
+      "curvature" : 0.09134825465286359
+    }, {
+      "time" : 1.2608381382277525,
+      "velocity" : 2.9944323642801653,
+      "acceleration" : -0.4439832537480419,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2447275870842858,
+          "y" : 2.8244726172701524
+        },
+        "rotation" : {
+          "radians" : -3.046760349874518
+        }
+      },
+      "curvature" : 0.10380826846683365
+    }, {
+      "time" : 1.2939625007283606,
+      "velocity" : 2.9797257020388157,
+      "acceleration" : -0.504652005340554,
+      "pose" : {
+        "translation" : {
+          "x" : 3.1462786868023294,
+          "y" : 2.814575920166238
+        },
+        "rotation" : {
+          "radians" : -3.0358170346264064
+        }
+      },
+      "curvature" : 0.11767714398773378
+    }, {
+      "time" : 1.3267346665866735,
+      "velocity" : 2.963187162819065,
+      "acceleration" : -0.5754544680210432,
+      "pose" : {
+        "translation" : {
+          "x" : 3.049505028486184,
+          "y" : 2.8037163393386826
+        },
+        "rotation" : {
+          "radians" : -3.023619364229805
+        }
+      },
+      "curvature" : 0.13316852630405346
+    }, {
+      "time" : 1.3591322565139792,
+      "velocity" : 2.9445438249422833,
+      "acceleration" : -0.6578485083677343,
+      "pose" : {
+        "translation" : {
+          "x" : 2.954548964348419,
+          "y" : 2.791821944706385
+        },
+        "rotation" : {
+          "radians" : -3.010063066623661
+        }
+      },
+      "curvature" : 0.1505299220368132
+    }, {
+      "time" : 1.391136692414039,
+      "velocity" : 2.923489754524278,
+      "acceleration" : -0.7533080854904565,
+      "pose" : {
+        "translation" : {
+          "x" : 2.8615516853041685,
+          "y" : 2.7788216520041806
+        },
+        "rotation" : {
+          "radians" : -2.995032698790502
+        }
+      },
+      "curvature" : 0.17004666410797128
+    }, {
+      "time" : 1.4227338667361988,
+      "velocity" : 2.8996873476287437,
+      "acceleration" : -0.8632025105219969,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7706524776540853,
+          "y" : 2.764645590590188
+        },
+        "rotation" : {
+          "radians" : -2.9784006000086296
+        }
+      },
+      "curvature" : 0.19204565508070712
+    }, {
+      "time" : 1.4539148256972156,
+      "velocity" : 2.8727718655731107,
+      "acceleration" : -3.0556739243578237,
+      "pose" : {
+        "translation" : {
+          "x" : 2.681987979767292,
+          "y" : 2.749225471253151
+        },
+        "rotation" : {
+          "radians" : -2.9600259471131536
+        }
+      },
+      "curvature" : 0.2168983389890578
+    }, {
+      "time" : 1.4850284729965166,
+      "velocity" : 2.7776987048289703,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5956914387643337,
+          "y" : 2.732494954019785
+        },
+        "rotation" : {
+          "radians" : -2.9397540138154308
+        }
+      },
+      "curvature" : 0.24502202477503635
+    }, {
+      "time" : 1.516611424846333,
+      "velocity" : 2.6513668974297047,
+      "acceleration" : -4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5118919672001288,
+          "y" : 2.7143900159621204
+        },
+        "rotation" : {
+          "radians" : -2.917415783997239
+        }
+      },
+      "curvature" : 0.2768782177504529
+    }, {
+      "time" : 1.548889355951825,
+      "velocity" : 2.5222551730077356,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.430713799746922,
+          "y" : 2.694849319004849
+        },
+        "rotation" : {
+          "radians" : -2.8928281326387415
+        }
+      },
+      "curvature" : 0.31296597525025704
+    }, {
+      "time" : 1.5819534964682787,
+      "velocity" : 2.3899986109419213,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3522755498772367,
+          "y" : 2.673814577732666
+        },
+        "rotation" : {
+          "radians" : -2.8657948699982057
+        }
+      },
+      "curvature" : 0.35380747084108527
+    }, {
+      "time" : 1.6159267882434154,
+      "velocity" : 2.254105443841374,
+      "acceleration" : -3.8097016292232664,
+      "pose" : {
+        "translation" : {
+          "x" : 2.276689466546827,
+          "y" : 2.6512309271976164
+        },
+        "rotation" : {
+          "radians" : -2.8361090449991986
+        }
+      },
+      "curvature" : 0.3999219560210479
+    }, {
+      "time" : 1.6509216012001868,
+      "velocity" : 2.120785647905598,
+      "acceleration" : -3.4461347634327937,
+      "pose" : {
+        "translation" : {
+          "x" : 2.204060690877629,
+          "y" : 2.62704729072644
+        },
+        "rotation" : {
+          "radians" : -2.80355701776569
+        }
+      },
+      "curvature" : 0.45178327798480244
+    }, {
+      "time" : 1.686971311060505,
+      "velocity" : 1.9965534895442902,
+      "acceleration" : -3.10238739377085,
+      "pose" : {
+        "translation" : {
+          "x" : 2.134486512840713,
+          "y" : 2.6012167477279142
+        },
+        "rotation" : {
+          "radians" : -2.7679249248735176
+        }
+      },
+      "curvature" : 0.5097553634159661
+    }, {
+      "time" : 1.7240545520371775,
+      "velocity" : 1.8815069102180944,
+      "acceleration" : -2.774924611544726,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0680556279392364,
+          "y" : 2.5736969015002
+        },
+        "rotation" : {
+          "radians" : -2.7290082446458435
+        }
+      },
+      "curvature" : 0.5740002323865068
+    }, {
+      "time" : 1.7621406045083514,
+      "velocity" : 1.7758209858592497,
+      "acceleration" : -2.460804023554924,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0048473938913958,
+          "y" : 2.544450247038185
+        },
+        "rotation" : {
+          "radians" : -2.6866251713458205
+        }
+      },
+      "curvature" : 0.6443551970164934
+    }, {
+      "time" : 1.8011869660776936,
+      "velocity" : 1.6797355422042324,
+      "acceleration" : -2.1575333813881774,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9449310873133778,
+          "y" : 2.5134445388408304
+        },
+        "rotation" : {
+          "radians" : -2.640634345688177
+        }
+      },
+      "curvature" : 0.7201813658904849
+    }, {
+      "time" : 1.841136715487461,
+      "velocity" : 1.5935426242745667,
+      "acceleration" : -1.86294906736094,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8883651604023122,
+          "y" : 2.480653158718514
+        },
+        "rotation" : {
+          "radians" : -2.590957058758506
+        }
+      },
+      "curvature" : 0.8001959234416516
+    }, {
+      "time" : 1.8819158229901707,
+      "velocity" : 1.5175732239845823,
+      "acceleration" : -1.5751064136624524,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8351964976192239,
+          "y" : 2.4460554836003747
+        },
+        "rotation" : {
+          "radians" : -2.537603239437523
+        }
+      },
+      "curvature" : 0.8823164840063602
+    }, {
+      "time" : 1.9234306511360026,
+      "velocity" : 1.452182951909988,
+      "acceleration" : -1.292173326268649,
+      "pose" : {
+        "translation" : {
+          "x" : 1.785459672371985,
+          "y" : 2.409637253341657
+        },
+        "rotation" : {
+          "radians" : -2.480699299573502
+        }
+      },
+      "curvature" : 0.9635650423352979
+    }, {
+      "time" : 1.9655659800403293,
+      "velocity" : 1.3977368038062605,
+      "acceleration" : -1.0123180093738429,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7391762036982668,
+          "y" : 2.3713909385310568
+        },
+        "rotation" : {
+          "radians" : -2.420514345602534
+        }
+      },
+      "curvature" : 1.0400947450439444
+    }, {
+      "time" : 2.008183951432712,
+      "velocity" : 1.3545938638427721,
+      "acceleration" : -0.733579462931417,
+      "pose" : {
+        "translation" : {
+          "x" : 1.696353812948491,
+          "y" : 2.331316108298065
+        },
+        "rotation" : {
+          "radians" : -2.357479728094081
+        }
+      },
+      "curvature" : 1.1074024899538168
+    }, {
+      "time" : 2.051124315824919,
+      "velocity" : 1.3230936943938578,
+      "acceleration" : -0.4537052298796883,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6569856804687841,
+          "y" : 2.2894197981203117
+        },
+        "rotation" : {
+          "radians" : -2.2921960475647642
+        }
+      },
+      "curvature" : 1.1607601882406264
+    }, {
+      "time" : 2.094206248814145,
+      "velocity" : 1.3035471960833196,
+      "acceleration" : -0.16993335972179005,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6210497022839263,
+          "y" : 2.2457168776309118
+        },
+        "rotation" : {
+          "radians" : -2.2254223695416244
+        }
+      },
+      "curvature" : 1.1958320363685782
+    }, {
+      "time" : 2.137231762276557,
+      "velocity" : 1.2962357260268969,
+      "acceleration" : 0.12131769723435752,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5885077467803068,
+          "y" : 2.200230418425809
+        },
+        "rotation" : {
+          "radians" : -2.1580450690428172
+        }
+      },
+      "curvature" : 1.2093603605633507
+    }, {
+      "time" : 2.1799904071245306,
+      "velocity" : 1.3014231063567148,
+      "acceleration" : 0.42491135071399705,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5593049113888746,
+          "y" : 2.152992061871121
+        },
+        "rotation" : {
+          "radians" : -2.091028180878493
+        }
+      },
+      "curvature" : 1.1997387249442901
+    }, {
+      "time" : 2.2222646301244318,
+      "velocity" : 1.3193859035519875,
+      "acceleration" : 0.7477114243725372,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5333687792680886,
+          "y" : 2.1040423869104825
+        },
+        "rotation" : {
+          "radians" : -2.0253520831348943
+        }
+      },
+      "curvature" : 1.1672933843552342
+    }, {
+      "time" : 2.2638349143884384,
+      "velocity" : 1.350468480010599,
+      "acceleration" : 1.099745716491029,
+      "pose" : {
+        "translation" : {
+          "x" : 1.510608675986873,
+          "y" : 2.0534312778723915
+        },
+        "rotation" : {
+          "radians" : -1.9619508454178411
+        }
+      },
+      "curvature" : 1.1141785653908227
+    }, {
+      "time" : 2.3044837782843635,
+      "velocity" : 1.3951718939603694,
+      "acceleration" : 1.4961728408552466,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4909149262075676,
+          "y" : 2.0012182922775534
+        },
+        "rotation" : {
+          "radians" : -1.9016590628319923
+        }
+      },
+      "curvature" : 1.0439225192180022
+    }, {
+      "time" : 2.343997831668839,
+      "velocity" : 1.4542917474663264,
+      "acceleration" : 1.9607687708811097,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4741581103688794,
+          "y" : 1.9474730286462254
+        },
+        "rotation" : {
+          "radians" : -1.8451762474768723
+        }
+      },
+      "curvature" : 0.9607726336453983
+    }, {
+      "time" : 2.3821673081822796,
+      "velocity" : 1.5291332650147609,
+      "acceleration" : 2.532455282243196,
+      "pose" : {
+        "translation" : {
+          "x" : 1.460188321368836,
+          "y" : 1.8922754943055606
+        },
+        "rotation" : {
+          "radians" : -1.7930521282820115
+        }
+      },
+      "curvature" : 0.869026522779364
+    }, {
+      "time" : 2.4187826706759017,
+      "velocity" : 1.6218600331729836,
+      "acceleration" : 3.8046696696170534,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4488344212477386,
+          "y" : 1.8357164731969533
+        },
+        "rotation" : {
+          "radians" : -1.745691473047995
+        }
+      },
+      "curvature" : 0.7724972819405648
+    }, {
+      "time" : 2.486114901209423,
+      "velocity" : 1.8780369284715344,
+      "acceleration" : 3.9999999999999947,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4331791216126497,
+          "y" : 1.7189331963567618
+        },
+        "rotation" : {
+          "radians" : -1.6662825767260925
+        }
+      },
+      "curvature" : 0.5761233113953619
+    }, {
+      "time" : 2.546692489588632,
+      "velocity" : 2.1203472819883706,
+      "acceleration" : 3.999999999999993,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4253702445836325,
+          "y" : 1.5980789786207197
+        },
+        "rotation" : {
+          "radians" : -1.608229977974804
+        }
+      },
+      "curvature" : 0.3843159112832851
+    }, {
+      "time" : 2.602172291072171,
+      "velocity" : 2.3422664879225272,
+      "acceleration" : 3.9999999999999893,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4231985572659944,
+          "y" : 1.4743055659818047
+        },
+        "rotation" : {
+          "radians" : -1.5723274201937065
+        }
+      },
+      "curvature" : 0.19618933375882694
+    }, {
+      "time" : 2.6534386588593746,
+      "velocity" : 2.54733195907134,
+      "acceleration" : 3.220008756110653,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4240437746047974,
+          "y" : 1.3489724397659302
+        },
+        "rotation" : {
+          "radians" : -1.5598679357771585
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 2.7013005707324047,
+      "velocity" : 2.7014477343866927,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4258348567705248,
+          "y" : 1.2233768946074477
+        },
+        "rotation" : {
+          "radians" : -1.550010895685447
+        }
+      },
+      "curvature" : 0.15261911450126764
+    }, {
+      "time" : 2.749505539118783,
+      "velocity" : 2.508627860841179,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4299988476074077,
+          "y" : 1.09787018620068
+        },
+        "rotation" : {
+          "radians" : -1.5225811069171706
+        }
+      },
+      "curvature" : 0.28021498367515013
+    }, {
+      "time" : 2.801724619491527,
+      "velocity" : 2.2997515393502033,
+      "acceleration" : -3.6618849413513943,
+      "pose" : {
+        "translation" : {
+          "x" : 1.438542295689782,
+          "y" : 0.972616643926834
+        },
+        "rotation" : {
+          "radians" : -1.4806280826538225
+        }
+      },
+      "curvature" : 0.3842039868268945
+    }, {
+      "time" : 2.858949646384186,
+      "velocity" : 2.090200075103547,
+      "acceleration" : -1.9233425253606922,
+      "pose" : {
+        "translation" : {
+          "x" : 1.45309450732384,
+          "y" : 0.8478549124047277
+        },
+        "rotation" : {
+          "radians" : -1.4270400417472306
+        }
+      },
+      "curvature" : 0.465101762848264
+    }, {
+      "time" : 2.920938902993327,
+      "velocity" : 1.9709735017516907,
+      "acceleration" : -0.9820932714498863,
+      "pose" : {
+        "translation" : {
+          "x" : 1.4749322121688557,
+          "y" : 0.7238891070967739
+        },
+        "rotation" : {
+          "radians" : -1.3645961892537415
+        }
+      },
+      "curvature" : 0.5230727944268867
+    }, {
+      "time" : 2.9861480086005843,
+      "velocity" : 1.906932077897538,
+      "acceleration" : -0.5051437890621265,
+      "pose" : {
+        "translation" : {
+          "x" : 1.505004228858411,
+          "y" : 0.6010799699149629
+        },
+        "rotation" : {
+          "radians" : -1.2959588931506363
+        }
+      },
+      "curvature" : 0.5587959564566138
+    }, {
+      "time" : 3.019621045984329,
+      "velocity" : 1.890023380962095,
+      "acceleration" : -0.2575060162536636,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5233410168520205,
+          "y" : 0.5402351909635741
+        },
+        "rotation" : {
+          "radians" : -1.2601011467472856
+        }
+      },
+      "curvature" : 0.5688389814615953
+    }, {
+      "time" : 3.0534660636487803,
+      "velocity" : 1.8813080852932873,
+      "acceleration" : -0.046118696833170436,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5439561306216198,
+          "y" : 0.4798360248268452
+        },
+        "rotation" : {
+          "radians" : -1.2236026771430364
+        }
+      },
+      "curvature" : 0.5741215645431921
+    }, {
+      "time" : 3.0875714219887946,
+      "velocity" : 1.8797351906116175,
+      "acceleration" : 0.13685239477635067,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5668869314298823,
+          "y" : 0.41993955191350785
+        },
+        "rotation" : {
+          "radians" : -1.1867261461606144
+        }
+      },
+      "curvature" : 0.575082775007997
+    }, {
+      "time" : 3.121837246466734,
+      "velocity" : 1.8844245507504096,
+      "acceleration" : 0.29657244735757804,
+      "pose" : {
+        "translation" : {
+          "x" : 1.5921549109043553,
+          "y" : 0.3606047334615141
+        },
+        "rotation" : {
+          "radians" : -1.1497095266580952
+        }
+      },
+      "curvature" : 0.5722241677945603
+    }, {
+      "time" : 3.15617469824573,
+      "velocity" : 1.8946080928605293,
+      "acceleration" : 0.43641554195126636,
+      "pose" : {
+        "translation" : {
+          "x" : 1.619766461838124,
+          "y" : 0.30189213515072333
+        },
+        "rotation" : {
+          "radians" : -1.1127638757054572
+        }
+      },
+      "curvature" : 0.566089275718927
+    }, {
+      "time" : 3.1905053847904026,
+      "velocity" : 1.9095905380344815,
+      "acceleration" : 0.5585224224331813,
+      "pose" : {
+        "translation" : {
+          "x" : 1.649713648990474,
+          "y" : 0.24386365071558913
+        },
+        "rotation" : {
+          "radians" : -1.076071959756705
+        }
+      },
+      "curvature" : 0.5572411700339329
+    }, {
+      "time" : 3.224760834869715,
+      "velocity" : 1.928722974994318,
+      "acceleration" : 0.6641677388812759,
+      "pose" : {
+        "translation" : {
+          "x" : 1.6819749798875545,
+          "y" : 0.18658222555784643
+        },
+        "rotation" : {
+          "radians" : -1.039787779550021
+        }
+      },
+      "curvature" : 0.5462406244811652
+    }, {
+      "time" : 3.258881991221138,
+      "velocity" : 1.9513851462562573,
+      "acceleration" : 0.7540110868217726,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7165161756230418,
+          "y" : 0.13011158035919834
+        },
+        "rotation" : {
+          "radians" : -1.0040369547627968
+        }
+      },
+      "curvature" : 0.5336268996944579
+    }, {
+      "time" : 3.2928186950481497,
+      "velocity" : 1.976973797192011,
+      "acceleration" : 0.8282759740654535,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7532909416588032,
+          "y" : 0.0745159346940032
+        },
+        "rotation" : {
+          "radians" : -0.9689178606738195
+        }
+      },
+      "curvature" : 0.5199024657633008
+    }, {
+      "time" : 3.3265291512596264,
+      "velocity" : 2.004895358146763,
+      "acceleration" : 0.8868837310332817,
+      "pose" : {
+        "translation" : {
+          "x" : 1.7922417386255596,
+          "y" : 0.01985973064196145
+        },
+        "rotation" : {
+          "radians" : -0.9345033658181425
+        }
+      },
+      "curvature" : 0.5055222588408043
+    }, {
+      "time" : 3.3599793731404124,
+      "velocity" : 2.0345618157322853,
+      "acceleration" : 0.9295593121848896,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8333005531235487,
+          "y" : -0.033792643599197314
+        },
+        "rotation" : {
+          "radians" : -0.9008430006823431
+        }
+      },
+      "curvature" : 0.4908874449734679
+    }, {
+      "time" : 3.3931426109904206,
+      "velocity" : 2.065389012297963,
+      "acceleration" : 0.9559197746209286,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8763896685231884,
+          "y" : -0.08637713010102743
+        },
+        "rotation" : {
+          "radians" : -0.8679653892038623
+        }
+      },
+      "curvature" : 0.4763432100815362
+    }, {
+      "time" : 3.425998772114722,
+      "velocity" : 2.0967968664348136,
+      "acceleration" : 0.96555227037989,
+      "pose" : {
+        "translation" : {
+          "x" : 1.921422435765741,
+          "y" : -0.1378302775916802
+        },
+        "rotation" : {
+          "radians" : -0.8358807916469844
+        }
+      },
+      "curvature" : 0.46217982712431704
+    }, {
+      "time" : 3.4585338402322123,
+      "velocity" : 2.1282111753226207,
+      "acceleration" : 0.9580857106502751,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9683040441639754,
+          "y" : -0.18808951784321692
+        },
+        "rotation" : {
+          "radians" : -0.8045836333638371
+        }
+      },
+      "curvature" : 0.44863614984773975
+    }, {
+      "time" : 3.4907393016272956,
+      "velocity" : 2.159066767690149,
+      "acceleration" : 0.9332583224551695,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0169322922028305,
+          "y" : -0.2370934420589219
+        },
+        "rotation" : {
+          "radians" : -0.774054923708963
+        }
+      },
+      "curvature" : 0.43590470633971296
+    }, {
+      "time" : 3.5226115837946828,
+      "velocity" : 2.1888118402785026,
+      "acceleration" : 0.8909818249589498,
+      "pose" : {
+        "translation" : {
+          "x" : 2.067198358340079,
+          "y" : -0.28478207726061555
+        },
+        "rotation" : {
+          "radians" : -0.744264498898693
+        }
+      },
+      "curvature" : 0.42413766870248887
+    }, {
+      "time" : 3.554151510376562,
+      "velocity" : 2.2169133416234965,
+      "acceleration" : 0.831401748795369,
+      "pose" : {
+        "translation" : {
+          "x" : 2.11898757180699,
+          "y" : -0.33109716267596734
+        },
+        "rotation" : {
+          "radians" : -0.7151730492255899
+        }
+      },
+      "curvature" : 0.41345311620209624
+    }, {
+      "time" : 3.5853637741835547,
+      "velocity" : 2.2428632723364927,
+      "acceleration" : 0.7549524443057307,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1721801834089938,
+          "y" : -0.3759824261258089
+        },
+        "rotation" : {
+          "radians" : -0.6867339133510525
+        }
+      },
+      "curvature" : 0.40394115698677635
+    }, {
+      "time" : 3.6162564282292005,
+      "velocity" : 2.266185757019344,
+      "acceleration" : 0.6624045708844728,
+      "pose" : {
+        "translation" : {
+          "x" : 2.226652136326343,
+          "y" : -0.41938386041144704
+        },
+        "rotation" : {
+          "radians" : -0.6588946399888784
+        }
+      },
+      "curvature" : 0.3956696075854153
+    }, {
+      "time" : 3.646840393102844,
+      "velocity" : 2.286444715147416,
+      "acceleration" : 0.5549023669290096,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2822758369147778,
+          "y" : -0.4612499997019768
+        },
+        "rotation" : {
+          "radians" : -0.6315983304138486
+        }
+      },
+      "curvature" : 0.38868903913170505
+    }, {
+      "time" : 3.677128977700188,
+      "velocity" : 2.3032519224314116,
+      "acceleration" : 0.43398782613727804,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3389209255061876,
+          "y" : -0.5015321959215944
+        },
+        "rotation" : {
+          "radians" : -0.6047847845266212
+        }
+      },
+      "curvature" : 0.38303708086136296
+    }, {
+      "time" : 3.7071374093201945,
+      "velocity" : 2.3162752164359675,
+      "acceleration" : 0.301609110666151,
+      "pose" : {
+        "translation" : {
+          "x" : 2.396455047209276,
+          "y" : -0.5401848951369104
+        },
+        "rotation" : {
+          "radians" : -0.5783914794951374
+        }
+      },
+      "curvature" : 0.37874192561228726
+    }, {
+      "time" : 3.736882368388014,
+      "velocity" : 2.3252465670872136,
+      "acceleration" : 0.16011115647032365,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4547446227102228,
+          "y" : -0.5771659139442626
+        },
+        "rotation" : {
+          "radians" : -0.5523544141104476
+        }
+      },
+      "curvature" : 0.37582501188792183
+    }, {
+      "time" : 3.766381522526342,
+      "velocity" : 2.3299697107711976,
+      "acceleration" : 0.012207476034279642,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5136556190733472,
+          "y" : -0.6124367158570294
+        },
+        "rotation" : {
+          "radians" : -0.5266088547091091
+        }
+      },
+      "curvature" : 0.37430286645269983
+    }, {
+      "time" : 3.7956530543152014,
+      "velocity" : 2.330327042293997,
+      "acceleration" : -0.13906640622183863,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5730543205417726,
+          "y" : -0.6459626876929423
+        },
+        "rotation" : {
+          "radians" : -0.5010900204761772
+        }
+      },
+      "curvature" : 0.37418808431592226
+    }, {
+      "time" : 3.8247151767948284,
+      "velocity" : 2.326285477363576,
+      "acceleration" : -0.29041573965154777,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6328080993380887,
+          "y" : -0.6777134159613993
+        },
+        "rotation" : {
+          "radians" : -0.47573374766730814
+        }
+      },
+      "curvature" : 0.37548940279405946
+    }, {
+      "time" : 3.8535856305237206,
+      "velocity" : 2.317901043189824,
+      "acceleration" : -0.4383615615686928,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6927861864650144,
+          "y" : -0.7076629632507778
+        },
+        "rotation" : {
+          "radians" : -0.45047717414862004
+        }
+      },
+      "curvature" : 0.37821079612304137
+    }, {
+      "time" : 3.882281155755939,
+      "velocity" : 2.305322027938995,
+      "acceleration" : -0.5793221339027272,
+      "pose" : {
+        "translation" : {
+          "x" : 2.752860442506062,
+          "y" : -0.7357901446157484
+        },
+        "rotation" : {
+          "radians" : -0.42525948787674483
+        }
+      },
+      "curvature" : 0.3823494796069915
+    }, {
+      "time" : 3.91081693300839,
+      "velocity" : 2.288790620568532,
+      "acceleration" : -0.7096933712661672,
+      "pose" : {
+        "translation" : {
+          "x" : 2.8129061284262007,
+          "y" : -0.7620788039645863
+        },
+        "rotation" : {
+          "radians" : -0.40002278560350163
+        }
+      },
+      "curvature" : 0.38789267058227345
+    }, {
+      "time" : 3.9392059849192336,
+      "velocity" : 2.2686430986108754,
+      "acceleration" : -0.8749184265202878,
+      "pose" : {
+        "translation" : {
+          "x" : 2.872802676372519,
+          "y" : -0.786518090446485
+        },
+        "rotation" : {
+          "radians" : -0.37471309109137346
+        }
+      },
+      "curvature" : 0.394812911527834
+    }, {
+      "time" : 3.9955941671938477,
+      "velocity" : 2.219308038900831,
+      "acceleration" : -1.0288336535097118,
+      "pose" : {
+        "translation" : {
+          "x" : 2.991691567646626,
+          "y" : -0.8298333259347146
+        },
+        "rotation" : {
+          "radians" : -0.3236861024053773
+        }
+      },
+      "curvature" : 0.4125613419519297
+    }, {
+      "time" : 4.051459900853216,
+      "velocity" : 2.161831492034062,
+      "acceleration" : -1.0796480258443715,
+      "pose" : {
+        "translation" : {
+          "x" : 3.108675287572691,
+          "y" : -0.865765651810257
+        },
+        "rotation" : {
+          "radians" : -0.2718791063461188
+        }
+      },
+      "curvature" : 0.43479047913732005
+    }, {
+      "time" : 4.10675359376877,
+      "velocity" : 2.10213376563614,
+      "acceleration" : -0.9994511460458044,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2230180775513873,
+          "y" : -0.8944474414456636
+        },
+        "rotation" : {
+          "radians" : -0.21916678625150354
+        }
+      },
+      "curvature" : 0.4598360409407436
+    }, {
+      "time" : 4.161311871060626,
+      "velocity" : 2.0476054328705096,
+      "acceleration" : -0.749178067288782,
+      "pose" : {
+        "translation" : {
+          "x" : 3.334124914760981,
+          "y" : -0.9161224852502343
+        },
+        "rotation" : {
+          "radians" : -0.16568957342253765
+        }
+      },
+      "curvature" : 0.48465328108684036
+    }, {
+      "time" : 4.214818370708727,
+      "velocity" : 2.007519536876757,
+      "acceleration" : -1.3699445774534182,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4415661777785544,
+          "y" : -0.9311548350640351
+        },
+        "rotation" : {
+          "radians" : -0.11197409892743718
+        }
+      },
+      "curvature" : 0.5042015108247184
+    }, {
+      "time" : 4.2675300331068176,
+      "velocity" : 1.9353074808059383,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5451023122012337,
+          "y" : -0.9400376485519146
+        },
+        "rotation" : {
+          "radians" : -0.059075560161144215
+        }
+      },
+      "curvature" : 0.5109123503903398
+    }, {
+      "time" : 4.322105290852558,
+      "velocity" : 1.7170064498229765,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6447084962674126,
+          "y" : -0.943402033597522
+        },
+        "rotation" : {
+          "radians" : -0.008719023872416791
+        }
+      },
+      "curvature" : 0.4946090622112777
+    }, {
+      "time" : 4.382159659747878,
+      "velocity" : 1.4767889742416929,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7405993064779786,
+          "y" : -0.9420258926973233
+        },
+        "rotation" : {
+          "radians" : 0.03660524430653648
+        }
+      },
+      "curvature" : 0.4434197350542565
+    }, {
+      "time" : 4.451511716406289,
+      "velocity" : 1.1993807476080485,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8332533832175386,
+          "y" : -0.9368427673546194
+        },
+        "rotation" : {
+          "radians" : 0.07365896092598179
+        }
+      },
+      "curvature" : 0.3462339716324929
+    }, {
+      "time" : 4.540069304785332,
+      "velocity" : 0.8451503940918763,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9234380963756443,
+          "y" : -0.9289506824735625
+        },
+        "rotation" : {
+          "radians" : 0.09866354717797672
+        }
+      },
+      "curvature" : 0.19694019866370868
+    }, {
+      "time" : 4.751356903308301,
+      "velocity" : 0.0,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 4.012234210968018,
+          "y" : -0.9196209907531738
+        },
+        "rotation" : {
+          "radians" : 0.10776957043787032
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.8985185623168945, -6.7758471965789795, 0.0 ],
+      "y" : [ 2.9429874420166016, -0.26568102836608887, 0.0 ]
+    }, {
+      "x" : [ 1.4240437746047974, 0.04392707347869873, 0.0 ],
+      "y" : [ 1.3489724397659302, -4.019377112388611, 0.0 ]
+    }, {
+      "x" : [ 4.012234210968018, 2.833451271057129, 0.0 ],
+      "y" : [ -0.9196209907531738, 0.30654752254486084, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.2617993877991494
+      }
+    }, {
+      "time" : 2.6499999407678843,
+      "rotation" : {
+        "radians" : 0.2617993877991494
+      }
+    }, {
+      "time" : 4.659999895840883,
+      "rotation" : {
+        "radians" : 0.2617993877991494
+      }
+    } ],
+    "reversed" : false,
+    "color" : 52.47058,
+    "closed" : true,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@t 0.89\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 0.89,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "REVERSE" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "EJECT" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.012234210968018,
+          "y" : -0.9196209907531738
+        },
+        "rotation" : {
+          "radians" : 0.030602586025410137
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.18393922011618816,
+      "velocity" : 0.7357568804647526,
+      "acceleration" : 3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 4.079868928493841,
+          "y" : -0.9175222116396733
+        },
+        "rotation" : {
+          "radians" : 0.03184996886547707
+        }
+      },
+      "curvature" : 0.03638401924476792
+    }, {
+      "time" : 0.26009263225753565,
+      "velocity" : 1.0403705290301426,
+      "acceleration" : 3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 4.1474601409882865,
+          "y" : -0.9152593300483591
+        },
+        "rotation" : {
+          "radians" : 0.035461574602982625
+        }
+      },
+      "curvature" : 0.06998452771616054
+    }, {
+      "time" : 0.3184715235418354,
+      "velocity" : 1.2738860941673413,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 4.214962774536021,
+          "y" : -0.9126807022060877
+        },
+        "rotation" : {
+          "radians" : 0.04125182940160068
+        }
+      },
+      "curvature" : 0.1010514081151579
+    }, {
+      "time" : 0.36761655293009865,
+      "velocity" : 1.4704662117203942,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 4.2823301801504385,
+          "y" : -0.9096479069208385
+        },
+        "rotation" : {
+          "radians" : 0.04904809314793145
+        }
+      },
+      "curvature" : 0.12981170585502438
+    }, {
+      "time" : 0.4108314858481353,
+      "velocity" : 1.643325943392541,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 4.349514283856005,
+          "y" : -0.9060353399753465
+        },
+        "rotation" : {
+          "radians" : 0.05868962955532622
+        }
+      },
+      "curvature" : 0.15646782863946646
+    }, {
+      "time" : 0.44980816479061647,
+      "velocity" : 1.7992326591624657,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.416465736770604,
+          "y" : -0.9017298085207326
+        },
+        "rotation" : {
+          "radians" : 0.07002652572535734
+        }
+      },
+      "curvature" : 0.1811979163878778
+    }, {
+      "time" : 0.4855501313208,
+      "velocity" : 1.9422005252831998,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.48313406518789,
+          "y" : -0.8966301254701357
+        },
+        "rotation" : {
+          "radians" : 0.08291864609124491
+        }
+      },
+      "curvature" : 0.20415755775322134
+    }, {
+      "time" : 0.5187103847104131,
+      "velocity" : 2.0748415388416523,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 4.549467820659629,
+          "y" : -0.8906467038923438
+        },
+        "rotation" : {
+          "radians" : 0.0972346733352272
+        }
+      },
+      "curvature" : 0.22548228467606032
+    }, {
+      "time" : 0.5497420218631486,
+      "velocity" : 2.1989680874525943,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 4.615414730078044,
+          "y" : -0.8837011514054263
+        },
+        "rotation" : {
+          "radians" : 0.1128512659439996
+        }
+      },
+      "curvature" : 0.24529045745559744
+    }, {
+      "time" : 0.5789746947138208,
+      "velocity" : 2.315898778855283,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 4.680921845758164,
+          "y" : -0.8757258645703647
+        },
+        "rotation" : {
+          "radians" : 0.12965234596402142
+        }
+      },
+      "curvature" : 0.263686278415705
+    }, {
+      "time" : 0.6066571501940509,
+      "velocity" : 2.4266286007762035,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.745935695520172,
+          "y" : -0.8666636232846845
+        },
+        "rotation" : {
+          "radians" : 0.14752851931001348
+        }
+      },
+      "curvature" : 0.2807627581933577
+    }, {
+      "time" : 0.6329825891472886,
+      "velocity" : 2.531930356589154,
+      "acceleration" : 0.9017846067562265,
+      "pose" : {
+        "translation" : {
+          "x" : 4.8104024327717525,
+          "y" : -0.8564671851760863
+        },
+        "rotation" : {
+          "radians" : 0.1663766232708782
+        }
+      },
+      "curvature" : 0.29660451731621684
+    }, {
+      "time" : 0.6584873043239601,
+      "velocity" : 2.5549301161351785,
+      "acceleration" : -2.1207024597033652,
+      "pose" : {
+        "translation" : {
+          "x" : 4.87426798659043,
+          "y" : -0.8450988799960781
+        },
+        "rotation" : {
+          "radians" : 0.18609939066967865
+        }
+      },
+      "curvature" : 0.3112903462074792
+    }, {
+      "time" : 0.683981890618659,
+      "velocity" : 2.500863684270891,
+      "acceleration" : -1.823821978528963,
+      "pose" : {
+        "translation" : {
+          "x" : 4.937478211805924,
+          "y" : -0.8325302040136062
+        },
+        "rotation" : {
+          "radians" : 0.20660521679714633
+        }
+      },
+      "curvature" : 0.3248954755335266
+    }, {
+      "time" : 0.7098179589134984,
+      "velocity" : 2.4537432950759874,
+      "acceleration" : -1.5828512441983833,
+      "pose" : {
+        "translation" : {
+          "x" : 4.999979039082493,
+          "y" : -0.8187414144086866
+        },
+        "rotation" : {
+          "radians" : 0.22780801329582814
+        }
+      },
+      "curvature" : 0.3374935302608176
+    }, {
+      "time" : 0.7359324293185369,
+      "velocity" : 2.4124079731037904,
+      "acceleration" : -1.3044976586946302,
+      "pose" : {
+        "translation" : {
+          "x" : 5.0617166250012815,
+          "y" : -0.8037211236660369
+        },
+        "rotation" : {
+          "radians" : 0.24962713231241265
+        }
+      },
+      "curvature" : 0.3491581576802025
+    }, {
+      "time" : 0.7887463969414447,
+      "velocity" : 2.343512275993333,
+      "acceleration" : -1.0334589786307198,
+      "pose" : {
+        "translation" : {
+          "x" : 5.182688729168589,
+          "y" : -0.7699798315917139
+        },
+        "rotation" : {
+          "radians" : 0.29481885303544775
+        }
+      },
+      "curvature" : 0.36998936253987214
+    }, {
+      "time" : 0.8420624190091038,
+      "velocity" : 2.288412354282637,
+      "acceleration" : -0.8513997934004414,
+      "pose" : {
+        "translation" : {
+          "x" : 5.299973998423866,
+          "y" : -0.7313669207204043
+        },
+        "rotation" : {
+          "radians" : 0.3416439837677253
+        }
+      },
+      "curvature" : 0.3880209156918146
+    }, {
+      "time" : 0.8955555645929367,
+      "velocity" : 2.242868301184222,
+      "acceleration" : -0.736658519915954,
+      "pose" : {
+        "translation" : {
+          "x" : 5.413165126823472,
+          "y" : -0.6880507100495148
+        },
+        "rotation" : {
+          "radians" : 0.389654433979741
+        }
+      },
+      "curvature" : 0.403939345595272
+    }, {
+      "time" : 0.9489629734818855,
+      "velocity" : 2.203525278399543,
+      "acceleration" : -0.6767969498415867,
+      "pose" : {
+        "translation" : {
+          "x" : 5.5218728309992,
+          "y" : -0.6402942652402999
+        },
+        "rotation" : {
+          "radians" : 0.43849160494770656
+        }
+      },
+      "curvature" : 0.41849245218579234
+    }, {
+      "time" : 1.0020763940584052,
+      "velocity" : 2.167578277357701,
+      "acceleration" : -0.6641591172113208,
+      "pose" : {
+        "translation" : {
+          "x" : 5.625730652793365,
+          "y" : -0.5884424192140703
+        },
+        "rotation" : {
+          "radians" : 0.48788654495705386
+        }
+      },
+      "curvature" : 0.4324880614534242
+    }, {
+      "time" : 1.0547362906352467,
+      "velocity" : 2.1326037269347866,
+      "acceleration" : -0.693255970599057,
+      "pose" : {
+        "translation" : {
+          "x" : 5.724399761893892,
+          "y" : -0.5329087927484011
+        },
+        "rotation" : {
+          "radians" : 0.5376585244703848
+        }
+      },
+      "curvature" : 0.4467899297328142
+    }, {
+      "time" : 1.1068266505846867,
+      "velocity" : 2.0964917738891833,
+      "acceleration" : -0.7590255374673471,
+      "pose" : {
+        "translation" : {
+          "x" : 5.817573758469408,
+          "y" : -0.4741628150733401
+        },
+        "rotation" : {
+          "radians" : 0.5877118587042041
+        }
+      },
+      "curvature" : 0.4623143546066941
+    }, {
+      "time" : 1.1582699222585076,
+      "velocity" : 2.0574450169578826,
+      "acceleration" : -0.85555708396171,
+      "pose" : {
+        "translation" : {
+          "x" : 5.904983475804329,
+          "y" : -0.4127167444676161
+        },
+        "rotation" : {
+          "radians" : 0.638030997754264
+        }
+      },
+      "curvature" : 0.4800287263774246
+    }, {
+      "time" : 1.20902166214638,
+      "velocity" : 2.014024006373431,
+      "acceleration" : -0.9751306653340864,
+      "pose" : {
+        "translation" : {
+          "x" : 5.986401782933953,
+          "y" : -0.3491126888548468
+        },
+        "rotation" : {
+          "radians" : 0.6886738736416058
+        }
+      },
+      "curvature" : 0.500950042603538
+    }, {
+      "time" : 1.259064520666594,
+      "velocity" : 1.9652256804493953,
+      "acceleration" : -1.1075634112905204,
+      "pose" : {
+        "translation" : {
+          "x" : 6.061648387279547,
+          "y" : -0.2839096263997476
+        },
+        "rotation" : {
+          "radians" : 0.7397632167792167
+        }
+      },
+      "curvature" : 0.5261369978659279
+    }, {
+      "time" : 1.3084011828915338,
+      "velocity" : 1.9105821985338527,
+      "acceleration" : -1.239915254926958,
+      "pose" : {
+        "translation" : {
+          "x" : 6.130594637283437,
+          "y" : -0.2176704261043394
+        },
+        "rotation" : {
+          "radians" : 0.7914750099875708
+        }
+      },
+      "curvature" : 0.5566628639559956
+    }, {
+      "time" : 1.3570458238963947,
+      "velocity" : 1.8502669660814803,
+      "acceleration" : -1.3565877307387766,
+      "pose" : {
+        "translation" : {
+          "x" : 6.193168325044098,
+          "y" : -0.1509488684041571
+        },
+        "rotation" : {
+          "radians" : 0.8440224046461846
+        }
+      },
+      "curvature" : 0.5935467253109423
+    }, {
+      "time" : 1.4050135519869393,
+      "velocity" : 1.7851945346824336,
+      "acceleration" : -1.439730441538962,
+      "pose" : {
+        "translation" : {
+          "x" : 6.249358488951245,
+          "y" : -0.0842766657644578
+        },
+        "rotation" : {
+          "radians" : 0.8976322459176475
+        }
+      },
+      "curvature" : 0.6376063101064825
+    }, {
+      "time" : 1.4523072062995137,
+      "velocity" : 1.7171044208769999,
+      "acceleration" : -1.4696274540546912,
+      "pose" : {
+        "translation" : {
+          "x" : 6.2992202163209186,
+          "y" : -0.018150483276428986
+        },
+        "rotation" : {
+          "radians" : 0.9525098498997762
+        }
+      },
+      "curvature" : 0.6891762313763988
+    }, {
+      "time" : 1.4989007546283921,
+      "velocity" : 1.6486292630710562,
+      "acceleration" : -1.4243277798746545,
+      "pose" : {
+        "translation" : {
+          "x" : 6.3428794460305795,
+          "y" : 0.04698104074660314
+        },
+        "rotation" : {
+          "radians" : 1.0087860125729582
+        }
+      },
+      "curvature" : 0.7476144640522491
+    }, {
+      "time" : 1.544718402885982,
+      "velocity" : 1.5833699138492454,
+      "acceleration" : -1.277051637682657,
+      "pose" : {
+        "translation" : {
+          "x" : 6.380537771154195,
+          "y" : 0.11073027417296544
+        },
+        "rotation" : {
+          "radians" : 1.0664389649669272
+        }
+      },
+      "curvature" : 0.8105110247651124
+    }, {
+      "time" : 1.589608352044505,
+      "velocity" : 1.5260431307608624,
+      "acceleration" : -0.9882350321042662,
+      "pose" : {
+        "translation" : {
+          "x" : 6.41247724159733,
+          "y" : 0.17278357045642956
+        },
+        "rotation" : {
+          "radians" : 1.1251844168307794
+        }
+      },
+      "curvature" : 0.8725495261033641
+    }, {
+      "time" : 1.6333097898626403,
+      "velocity" : 1.482855838955655,
+      "acceleration" : -0.4841929472411742,
+      "pose" : {
+        "translation" : {
+          "x" : 6.4390651667322345,
+          "y" : 0.2329142480400037
+        },
+        "rotation" : {
+          "radians" : 1.1843313194749898
+        }
+      },
+      "curvature" : 0.9241146185221707
+    }, {
+      "time" : 1.6754107043468742,
+      "velocity" : 1.4624708730899851,
+      "acceleration" : -3.7808476405994242,
+      "pose" : {
+        "translation" : {
+          "x" : 6.460758918032937,
+          "y" : 0.2909955697597244
+        },
+        "rotation" : {
+          "radians" : 1.2426127877010575
+        }
+      },
+      "curvature" : 0.9500561047849169
+    }, {
+      "time" : 1.7178366346797787,
+      "velocity" : 1.3020648944905882,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.478110731710331,
+          "y" : 0.3470137222484482
+        },
+        "rotation" : {
+          "radians" : 1.2980233032835509
+        }
+      },
+      "curvature" : 0.9295171145481738
+    }, {
+      "time" : 1.7639291544880524,
+      "velocity" : 1.117694815257493,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 6.491772511347264,
+          "y" : 0.40108079533964336
+        },
+        "rotation" : {
+          "radians" : 1.3477231257633573
+        }
+      },
+      "curvature" : 0.838208547283802
+    }, {
+      "time" : 1.8167467720854111,
+      "velocity" : 0.9064243448680579,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.502500630533632,
+          "y" : 0.4534477614711818
+        },
+        "rotation" : {
+          "radians" : 1.3880989582767027
+        }
+      },
+      "curvature" : 0.6544921319595709
+    }, {
+      "time" : 2.0433528583024256,
+      "velocity" : 0.0,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.51873254776001,
+          "y" : 0.5548575520515442
+        },
+        "rotation" : {
+          "radians" : 1.4247813178399886
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 4.012234210968018, 4.329079627990723, 0.0 ],
+      "y" : [ -0.9196209907531738, 0.13252240419387817, 0.0 ]
+    }, {
+      "x" : [ 6.51873254776001, 0.23627901077270508, 0.0 ],
+      "y" : [ 0.5548575520515442, 1.6066665649414062, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : -0.13962634015954636
+      }
+    }, {
+      "time" : 1.9499999564141035,
+      "rotation" : {
+        "radians" : -0.1526493858119171
+      }
+    } ],
+    "reversed" : false,
+    "color" : 286.3529,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "sleep 2000",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "sleep",
+        "args" : [ "2000" ],
+        "argTypes" : [ "long" ],
+        "reflection" : false
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 6.51873254776001,
+          "y" : 0.5548575520515442
+        },
+        "rotation" : {
+          "radians" : 1.4550369939781984
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.1788531666911449,
+      "velocity" : 0.7154126667645795,
+      "acceleration" : 3.999999999999994,
+      "pose" : {
+        "translation" : {
+          "x" : 6.526029236446348,
+          "y" : 0.6184169998749507
+        },
+        "rotation" : {
+          "radians" : 1.4593465391163816
+        }
+      },
+      "curvature" : 0.13050101301586398
+    }, {
+      "time" : 0.25321186756390435,
+      "velocity" : 1.012847470255617,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.532831722088758,
+          "y" : 0.6823114984076142
+        },
+        "rotation" : {
+          "radians" : 1.471236108776517
+        }
+      },
+      "curvature" : 0.23504966165445965
+    }, {
+      "time" : 0.3106349893777712,
+      "velocity" : 1.2425399575110843,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.538718402694414,
+          "y" : 0.746799069553143
+        },
+        "rotation" : {
+          "radians" : 1.4891403961437595
+        }
+      },
+      "curvature" : 0.31358925447041286
+    }, {
+      "time" : 0.3594524552162631,
+      "velocity" : 1.437809820865052,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 6.543342575678253,
+          "y" : 0.8120593882322282
+        },
+        "rotation" : {
+          "radians" : 1.511568465726473
+        }
+      },
+      "curvature" : 0.3681705103657191
+    }, {
+      "time" : 0.4028800665262472,
+      "velocity" : 1.611520266104988,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 6.546427705950194,
+          "y" : 0.8782000352682129
+        },
+        "rotation" : {
+          "radians" : 1.5371749251346154
+        }
+      },
+      "curvature" : 0.4020595428820239
+    }, {
+      "time" : 0.44254982612556115,
+      "velocity" : 1.7701993045022437,
+      "acceleration" : 3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 6.547762694002358,
+          "y" : 0.9452627502726614
+        },
+        "rotation" : {
+          "radians" : 1.5647989424802173
+        }
+      },
+      "curvature" : 0.41904605440094195
+    }, {
+      "time" : 0.4794110889330761,
+      "velocity" : 1.9176443557323033,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.547197143996286,
+          "y" : 1.0132296845309288
+        },
+        "rotation" : {
+          "radians" : 1.5934786052963947
+        }
+      },
+      "curvature" : 0.42293973727499673
+    }, {
+      "time" : 0.5140610781999098,
+      "velocity" : 2.056244312799638,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.544636631850153,
+          "y" : 1.08202965388773
+        },
+        "rotation" : {
+          "radians" : 1.622447172268418
+        }
+      },
+      "curvature" : 0.41723748069828426
+    }, {
+      "time" : 0.5468931637504397,
+      "velocity" : 2.187572655001758,
+      "acceleration" : 3.1633627206418256,
+      "pose" : {
+        "translation" : {
+          "x" : 6.540037973325994,
+          "y" : 1.1515443916327097
+        },
+        "rotation" : {
+          "radians" : 1.651117099798838
+        }
+      },
+      "curvature" : 0.4049376530908519
+    }, {
+      "time" : 0.5783519506192766,
+      "velocity" : 2.287088208619253,
+      "acceleration" : 1.870434307392289,
+      "pose" : {
+        "translation" : {
+          "x" : 6.533404492116915,
+          "y" : 1.2216148013860106
+        },
+        "rotation" : {
+          "radians" : 1.6790569837278395
+        }
+      },
+      "curvature" : 0.38847034737186514
+    }, {
+      "time" : 0.608993635935249,
+      "velocity" : 2.3444014680705667,
+      "acceleration" : 2.165397894604544,
+      "pose" : {
+        "translation" : {
+          "x" : 6.524781287934317,
+          "y" : 1.2920472099838438
+        },
+        "rotation" : {
+          "radians" : 1.7059655876740503
+        }
+      },
+      "curvature" : 0.36970875427049765
+    }, {
+      "time" : 0.6390132720691309,
+      "velocity" : 2.409405924951669,
+      "acceleration" : 2.411756982149806,
+      "pose" : {
+        "translation" : {
+          "x" : 6.514250504595111,
+          "y" : 1.3626196203640575
+        },
+        "rotation" : {
+          "radians" : 1.731646007957078
+        }
+      },
+      "curvature" : 0.3500287810963245
+    }, {
+      "time" : 0.668275787449217,
+      "velocity" : 2.479980000734858,
+      "acceleration" : 2.6202614568139766,
+      "pose" : {
+        "translation" : {
+          "x" : 6.501926598108938,
+          "y" : 1.4330879644517065
+        },
+        "rotation" : {
+          "radians" : 1.7559819224996793
+        }
+      },
+      "curvature" : 0.3303903442679772
+    }, {
+      "time" : 0.6966740713701192,
+      "velocity" : 2.554390929532458,
+      "acceleration" : 2.7997101251293675,
+      "pose" : {
+        "translation" : {
+          "x" : 6.487951604765385,
+          "y" : 1.5031923560446216
+        },
+        "rotation" : {
+          "radians" : 1.7789169353574112
+        }
+      },
+      "curvature" : 0.3114217758149773
+    }, {
+      "time" : 0.7241232587242202,
+      "velocity" : 2.6312406972943077,
+      "acceleration" : 1.2935503366228165,
+      "pose" : {
+        "translation" : {
+          "x" : 6.4724904092212086,
+          "y" : 1.572663343698979
+        },
+        "rotation" : {
+          "radians" : 1.8004373323306622
+        }
+      },
+      "curvature" : 0.2934962442471143
+    }, {
+      "time" : 0.7507742479488775,
+      "velocity" : 2.665715093377194,
+      "acceleration" : -3.999999999999994,
+      "pose" : {
+        "translation" : {
+          "x" : 6.455726012587547,
+          "y" : 1.6412281636148691
+        },
+        "rotation" : {
+          "radians" : 1.8205581086255052
+        }
+      },
+      "curvature" : 0.27679481544391576
+    }, {
+      "time" : 0.7774623046076926,
+      "velocity" : 2.5589628667419335,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 6.437854800517144,
+          "y" : 1.7086169925218666
+        },
+        "rotation" : {
+          "radians" : 1.8393118764446346
+        }
+      },
+      "curvature" : 0.2613545325842005
+    }, {
+      "time" : 0.8048451310611667,
+      "velocity" : 2.449431560928037,
+      "acceleration" : -3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 6.419081811291562,
+          "y" : 1.7745692005645992
+        },
+        "rotation" : {
+          "radians" : 1.856740154150982
+        }
+      },
+      "curvature" : 0.24710198818221107
+    }, {
+      "time" : 0.8329039549239173,
+      "velocity" : 2.3371962654770346,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.399616003908406,
+          "y" : 1.8388396041883173
+        },
+        "rotation" : {
+          "radians" : 1.8728865291753276
+        }
+      },
+      "curvature" : 0.23387381346481986
+    }, {
+      "time" : 0.861625693206511,
+      "velocity" : 2.22230931234666,
+      "acceleration" : -4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 6.379665526168537,
+          "y" : 1.901204719024463
+        },
+        "rotation" : {
+          "radians" : 1.8877912376445647
+        }
+      },
+      "curvature" : 0.22142585640379966
+    }, {
+      "time" : 0.921065056241802,
+      "velocity" : 1.9845518602054957,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.339110703361712,
+          "y" : 2.0194711581041815
+        },
+        "rotation" : {
+          "radians" : 1.9139944386552825
+        }
+      },
+      "curvature" : 0.19748286503535473
+    }, {
+      "time" : 0.9834254421957473,
+      "velocity" : 1.7351103163897141,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 6.298886488657445,
+          "y" : 2.1282522362307645
+        },
+        "rotation" : {
+          "radians" : 1.9354586912044225
+        }
+      },
+      "curvature" : 0.17157685598367362
+    }, {
+      "time" : 1.0497222979942022,
+      "velocity" : 1.4699228931958945,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.260146206276204,
+          "y" : 2.2271790434867285
+        },
+        "rotation" : {
+          "radians" : 1.9520508222111153
+        }
+      },
+      "curvature" : 0.13862722514403217
+    }, {
+      "time" : 1.1228723260834714,
+      "velocity" : 1.1773227808388174,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 6.223575937663554,
+          "y" : 2.316830133948315
+        },
+        "rotation" : {
+          "radians" : 1.9634500574230802
+        }
+      },
+      "curvature" : 0.09407105630824328
+    }, {
+      "time" : 1.2119347577273158,
+      "velocity" : 0.8210730542634398,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.189243100281146,
+          "y" : 2.3989316180237097
+        },
+        "rotation" : {
+          "radians" : 1.969501422562912
+        }
+      },
+      "curvature" : 0.0409346875176687
+    }, {
+      "time" : 1.4172030212931759,
+      "velocity" : 0.0,
+      "acceleration" : -3.9999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 6.156445026397705,
+          "y" : 2.4765572547912598
+        },
+        "rotation" : {
+          "radians" : 1.9710366419843302
+        }
+      },
+      "curvature" : -0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 6.51873254776001, 0.23627471923828125, 0.0 ],
+      "y" : [ 0.5548575520515442, 2.0319605469703674, 0.0 ]
+    }, {
+      "x" : [ 6.156445026397705, -0.519803524017334, 0.0 ],
+      "y" : [ 2.4765572547912598, 1.2286276817321777, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.4704038872114269
+      }
+    }, {
+      "time" : 1.3299999702721834,
+      "rotation" : {
+        "radians" : 0.8760571821323136
+      }
+    } ],
+    "reversed" : false,
+    "color" : 74.117645,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  } ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/autos/red/movecargotohanger.json b/src/main/deploy/autos/red/movecargotohanger.json
new file mode 100644
index 00000000..7607216c
--- /dev/null
+++ b/src/main/deploy/autos/red/movecargotohanger.json
@@ -0,0 +1,3505 @@
+{
+  "autonomousSteps" : [ {
+    "type" : "script",
+    "script" : "Intake.setIntakeSolState OPEN\nIntake.setWantedIntakeState INTAKE\nHopper.setHopperState ON",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setIntakeSolState",
+        "args" : [ "OPEN" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeSolState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "INTAKE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "ON" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "@t 0.01\nDrive.setAutoRotation 173",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 0.01,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Drive.setAutoRotation",
+        "args" : [ "173" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 6.6141839027404785,
+          "y" : 1.5894602537155151
+        },
+        "rotation" : {
+          "radians" : 2.389889403743233
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.2489252451296045,
+      "velocity" : 0.995700980518418,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 6.523176953443453,
+          "y" : 1.6735778746319738
+        },
+        "rotation" : {
+          "radians" : 2.4064673708251867
+        }
+      },
+      "curvature" : 0.25911994232595476
+    }, {
+      "time" : 0.3496677436327589,
+      "velocity" : 1.3986709745310357,
+      "acceleration" : 3.999999999999997,
+      "pose" : {
+        "translation" : {
+          "x" : 6.432098919045529,
+          "y" : 1.7526409019337734
+        },
+        "rotation" : {
+          "radians" : 2.4514777463969604
+        }
+      },
+      "curvature" : 0.48603924486171285
+    }, {
+      "time" : 0.4240995092316322,
+      "velocity" : 1.6963980369265288,
+      "acceleration" : -2.790576412342658,
+      "pose" : {
+        "translation" : {
+          "x" : 6.340960895459375,
+          "y" : 1.823079613135178
+        },
+        "rotation" : {
+          "radians" : 2.519876340424447
+        }
+      },
+      "curvature" : 0.701310596977904
+    }, {
+      "time" : 0.49210983031570427,
+      "velocity" : 1.5066100391134667,
+      "acceleration" : -1.7711769053800082,
+      "pose" : {
+        "translation" : {
+          "x" : 6.249847903382033,
+          "y" : 1.8827598318457603
+        },
+        "rotation" : {
+          "radians" : 2.607059147103057
+        }
+      },
+      "curvature" : 0.895203949702412
+    }, {
+      "time" : 0.5274607298209384,
+      "velocity" : 1.4439973423253865,
+      "acceleration" : -1.205251380120216,
+      "pose" : {
+        "translation" : {
+          "x" : 6.204343319524796,
+          "y" : 1.9082412143392418
+        },
+        "rotation" : {
+          "radians" : 2.6558796477618074
+        }
+      },
+      "curvature" : 0.9745203573253068
+    }, {
+      "time" : 0.563119080457446,
+      "velocity" : 1.401020066007925,
+      "acceleration" : -0.687574960710818,
+      "pose" : {
+        "translation" : {
+          "x" : 6.15890158834145,
+          "y" : 1.9307801831205325
+        },
+        "rotation" : {
+          "radians" : 2.706939505672123
+        }
+      },
+      "curvature" : 1.0352255751207677
+    }, {
+      "time" : 0.5987086013693859,
+      "velocity" : 1.376549602565181,
+      "acceleration" : -0.17765980611680962,
+      "pose" : {
+        "translation" : {
+          "x" : 6.113546424879203,
+          "y" : 1.9504234287532398
+        },
+        "rotation" : {
+          "radians" : 2.7591326119824173
+        }
+      },
+      "curvature" : 1.0723584365876502
+    }, {
+      "time" : 0.6338600425589757,
+      "velocity" : 1.3703046043387122,
+      "acceleration" : 0.36242756952241223,
+      "pose" : {
+        "translation" : {
+          "x" : 6.068302920743008,
+          "y" : 1.9672693488100776
+        },
+        "rotation" : {
+          "radians" : 2.81126068578529
+        }
+      },
+      "curvature" : 1.0821549980493446
+    }, {
+      "time" : 0.6682116558661673,
+      "velocity" : 1.3827545760588114,
+      "acceleration" : 0.9785292578911144,
+      "pose" : {
+        "translation" : {
+          "x" : 6.023197003472021,
+          "y" : 1.9814617121025577
+        },
+        "rotation" : {
+          "radians" : 2.862095726190118
+        }
+      },
+      "curvature" : 1.0627558269737196
+    }, {
+      "time" : 0.7014108163316237,
+      "velocity" : 1.4152409259116825,
+      "acceleration" : 2.2441271164822805,
+      "pose" : {
+        "translation" : {
+          "x" : 5.978254895916052,
+          "y" : 1.9931833229106815
+        },
+        "rotation" : {
+          "radians" : 2.91045196330826
+        }
+      },
+      "curvature" : 1.0145254489555913
+    }, {
+      "time" : 0.7626515005088358,
+      "velocity" : 1.5526728059056913,
+      "acceleration" : 3.9999999999999925,
+      "pose" : {
+        "translation" : {
+          "x" : 5.888965234160423,
+          "y" : 2.010102666914463
+        },
+        "rotation" : {
+          "radians" : 2.9955886329173094
+        }
+      },
+      "curvature" : 0.8428762374068068
+    }, {
+      "time" : 0.8162079335949602,
+      "velocity" : 1.7668985382501887,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.800629080792987,
+          "y" : 2.0200297651595065
+        },
+        "rotation" : {
+          "radians" : 3.0601867848614575
+        }
+      },
+      "curvature" : 0.6011409133906767
+    }, {
+      "time" : 0.8631605793414101,
+      "velocity" : 1.9547091212359882,
+      "acceleration" : -2.958595604807148,
+      "pose" : {
+        "translation" : {
+          "x" : 5.713411706194165,
+          "y" : 2.025186080179992
+        },
+        "rotation" : {
+          "radians" : 3.1009099300153964
+        }
+      },
+      "curvature" : 0.3280169918222083
+    }, {
+      "time" : 0.9087395705485002,
+      "velocity" : 1.8198593181791478,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 5.627431205854464,
+          "y" : 2.027809408056328
+        },
+        "rotation" : {
+          "radians" : 3.1173842319085643
+        }
+      },
+      "curvature" : 0.05731406395107486
+    }, {
+      "time" : 0.957952684084819,
+      "velocity" : 1.6230068640338726,
+      "acceleration" : -4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 5.5427412004210055,
+          "y" : 2.0299511337652802
+        },
+        "rotation" : {
+          "radians" : 3.1118848820659286
+        }
+      },
+      "curvature" : -0.17922803729629205
+    }, {
+      "time" : 1.0131511813558638,
+      "velocity" : 1.4022128749496938,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.459313535744059,
+          "y" : 2.0332734865301063
+        },
+        "rotation" : {
+          "radians" : 3.0893774981886746
+        }
+      },
+      "curvature" : -0.3450488004527902
+    }, {
+      "time" : 1.077964961333034,
+      "velocity" : 1.1429577550410128,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.377020982923568,
+          "y" : 2.0388467951706843
+        },
+        "rotation" : {
+          "radians" : 3.0578354549476097
+        }
+      },
+      "curvature" : -0.3971872754778253
+    }, {
+      "time" : 1.161849251134293,
+      "velocity" : 0.8074205958359769,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.295619938355685,
+          "y" : 2.0469467434536455
+        },
+        "rotation" : {
+          "radians" : 3.0283983068361184
+        }
+      },
+      "curvature" : -0.2932585838727471
+    }, {
+      "time" : 1.3637044000932872,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.214733123779297,
+          "y" : 2.056851625442505
+        },
+        "rotation" : {
+          "radians" : 3.0150708953014127
+        }
+      },
+      "curvature" : -0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 6.6141839027404785, -1.4558343887329102, 0.0 ],
+      "y" : [ 1.5894602537155151, 1.360889196395874, 0.0 ]
+    }, {
+      "x" : [ 5.214733123779297, -1.292677402496338, 0.0 ],
+      "y" : [ 2.056851625442505, 0.1644301414489746, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.3941697598804894
+      }
+    }, {
+      "time" : 1.269999971613288,
+      "rotation" : {
+        "radians" : 3.0222255927586548
+      }
+    } ],
+    "reversed" : false,
+    "color" : 209.88235,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "# We set it to shoot 10 so that even in the case of a small jam we still shoot\nVisionManager.shootBalls 10",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.VisionManager.shootBalls",
+        "args" : [ "10" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "Hopper.setEjectOverride true\nDrive.turnToAngle 45",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "NONE",
+      "delay" : 0.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setEjectOverride",
+        "args" : [ "true" ],
+        "argTypes" : [ "boolean" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Drive.turnToAngle",
+        "args" : [ "45" ],
+        "argTypes" : [ "double" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.214733123779297,
+          "y" : 2.056851625442505
+        },
+        "rotation" : {
+          "radians" : 0.9530775920796433
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.22164892589636578,
+      "velocity" : 0.8865957035854634,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 5.272283410420641,
+          "y" : 2.136490201934066
+        },
+        "rotation" : {
+          "radians" : 0.93154333853559
+        }
+      },
+      "curvature" : -0.3354703724748357
+    }, {
+      "time" : 0.32212126730993046,
+      "velocity" : 1.2884850692397223,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 5.339010516181588,
+          "y" : 2.223017249489203
+        },
+        "rotation" : {
+          "radians" : 0.8980578131179813
+        }
+      },
+      "curvature" : -0.24297628723974596
+    }, {
+      "time" : 0.40798632695507736,
+      "velocity" : 1.6319453078203097,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.418256742414087,
+          "y" : 2.320179727881623
+        },
+        "rotation" : {
+          "radians" : 0.8787668506583872
+        }
+      },
+      "curvature" : -0.07195280186555318
+    }, {
+      "time" : 0.4482103474158235,
+      "velocity" : 1.7928413896632942,
+      "acceleration" : 4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 5.462294308261335,
+          "y" : 2.373142516471262
+        },
+        "rotation" : {
+          "radians" : 0.8763509051285007
+        }
+      },
+      "curvature" : -0.0010310805010032027
+    }, {
+      "time" : 0.48701783511066765,
+      "velocity" : 1.9480713404426708,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.508707731962204,
+          "y" : 2.428952641785145
+        },
+        "rotation" : {
+          "radians" : 0.8784798678297706
+        }
+      },
+      "curvature" : 0.057187573384384974
+    }, {
+      "time" : 0.5244749433253648,
+      "velocity" : 2.0978997733014593,
+      "acceleration" : -3.006834100609329,
+      "pose" : {
+        "translation" : {
+          "x" : 5.5569134053257585,
+          "y" : 2.4874170951864016
+        },
+        "rotation" : {
+          "radians" : 0.8846611837531144
+        }
+      },
+      "curvature" : 0.10384088533571874
+    }, {
+      "time" : 0.5628765788352517,
+      "velocity" : 1.9824324261311614,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 5.606271566124633,
+          "y" : 2.5482597037116648
+        },
+        "rotation" : {
+          "radians" : 0.8942836471940969
+        }
+      },
+      "curvature" : 0.13988861038856043
+    }, {
+      "time" : 0.6051667706790917,
+      "velocity" : 1.813271658755801,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.656145019875112,
+          "y" : 2.6111436507878807
+        },
+        "rotation" : {
+          "radians" : 0.9066044762714179
+        }
+      },
+      "curvature" : 0.16515378657020438
+    }, {
+      "time" : 0.6526161179857626,
+      "velocity" : 1.6234742695291178,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.7059578616172075,
+          "y" : 2.6756939969491214
+        },
+        "rotation" : {
+          "radians" : 0.9206836126942823
+        }
+      },
+      "curvature" : 0.17781422207911715
+    }, {
+      "time" : 0.7069026874415512,
+      "velocity" : 1.4063279917059635,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 5.75525419769474,
+          "y" : 2.7415202005533956
+        },
+        "rotation" : {
+          "radians" : 0.9352834691742239
+        }
+      },
+      "curvature" : 0.1740904256082478
+    }, {
+      "time" : 0.7714879126896856,
+      "velocity" : 1.1479870907134255,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 5.803756867535412,
+          "y" : 2.8082386384994606
+        },
+        "rotation" : {
+          "radians" : 0.9487507880978892
+        }
+      },
+      "curvature" : 0.14816445891139096
+    }, {
+      "time" : 0.8556329295191124,
+      "velocity" : 0.8114070233957184,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.851426165430894,
+          "y" : 2.8754951269436333
+        },
+        "rotation" : {
+          "radians" : 0.9589069733276255
+        }
+      },
+      "curvature" : 0.09262519823750537
+    }, {
+      "time" : 1.058484685368042,
+      "velocity" : 0.0,
+      "acceleration" : -3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 5.8985185623168945,
+          "y" : 2.9429874420166016
+        },
+        "rotation" : {
+          "radians" : 0.9629949901604784
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.214733123779297, 0.4449276924133301, 0.0 ],
+      "y" : [ 2.056851625442505, 0.6262435913085938, 0.0 ]
+    }, {
+      "x" : [ 5.8985185623168945, 0.7516036033630371, 0.0 ],
+      "y" : [ 2.9429874420166016, 1.0804316997528076, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 0.9599310885968813
+      }
+    }, {
+      "time" : 0.9699999783188105,
+      "rotation" : {
+        "radians" : 0.9599310885968813
+      }
+    } ],
+    "reversed" : false,
+    "color" : 148.47061,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@t 1\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 1.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "REVERSE" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "EJECT" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "@t 1.6\nHopper.setHopperState ON\nIntake.setWantedIntakeState INTAKE",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 1.6,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "ON" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "INTAKE" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.8985185623168945,
+          "y" : 2.9429874420166016
+        },
+        "rotation" : {
+          "radians" : -3.1024027203970066
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 0.2301659590513677,
+      "velocity" : 0.9206638362054708,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.792647884092066,
+          "y" : 2.9388182625618144
+        },
+        "rotation" : {
+          "radians" : -3.1018966911012167
+        }
+      },
+      "curvature" : 0.009441605420194805
+    }, {
+      "time" : 0.32549598090329745,
+      "velocity" : 1.3019839236131898,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.686791604118646,
+          "y" : 2.9345442011023373
+        },
+        "rotation" : {
+          "radians" : -3.100425032373633
+        }
+      },
+      "curvature" : 0.01823558838667193
+    }, {
+      "time" : 0.3986288451113676,
+      "velocity" : 1.5945153804454704,
+      "acceleration" : 3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 5.580971612904613,
+          "y" : 2.9300674260512527
+        },
+        "rotation" : {
+          "radians" : -3.098055153290077
+        }
+      },
+      "curvature" : 0.026416758928169817
+    }, {
+      "time" : 0.4602561477805154,
+      "velocity" : 1.8410245911220613,
+      "acceleration" : 3.9999999999999916,
+      "pose" : {
+        "translation" : {
+          "x" : 5.475217593972275,
+          "y" : 2.9252976053729753
+        },
+        "rotation" : {
+          "radians" : -3.094851059223684
+        }
+      },
+      "curvature" : 0.0340260625248394
+    }, {
+      "time" : 0.5145132056228292,
+      "velocity" : 2.0580528224913164,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.369566605466148,
+          "y" : 2.9201516955883897
+        },
+        "rotation" : {
+          "radians" : -3.090872945078929
+        }
+      },
+      "curvature" : 0.04110875647986678
+    }, {
+      "time" : 0.563515762369635,
+      "velocity" : 2.2540630494785394,
+      "acceleration" : 4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 5.264062661760839,
+          "y" : 2.914553730779989
+        },
+        "rotation" : {
+          "radians" : -3.086176913297493
+        }
+      },
+      "curvature" : 0.047712969032265094
+    }, {
+      "time" : 0.6085162323222143,
+      "velocity" : 2.4340649292888568,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.158756315068919,
+          "y" : 2.9084346115970146
+        },
+        "rotation" : {
+          "radians" : -3.080814792156677
+        }
+      },
+      "curvature" : 0.05388859471718488
+    }, {
+      "time" : 0.6503267256223321,
+      "velocity" : 2.601306902489328,
+      "acceleration" : 3.9999999999999956,
+      "pose" : {
+        "translation" : {
+          "x" : 5.053704237048805,
+          "y" : 2.9017318942605925
+        },
+        "rotation" : {
+          "radians" : -3.0748340330236092
+        }
+      },
+      "curvature" : 0.059686480108153514
+    }, {
+      "time" : 0.6895078614844453,
+      "velocity" : 2.758031445937781,
+      "acceleration" : 3.9999999999999956,
+      "pose" : {
+        "translation" : {
+          "x" : 4.948968800412642,
+          "y" : 2.894389579568873
+        },
+        "rotation" : {
+          "radians" : -3.0682776682148387
+        }
+      },
+      "curvature" : 0.06515785747811133
+    }, {
+      "time" : 0.7264647120959763,
+      "velocity" : 2.9058588483839047,
+      "acceleration" : 2.666249486168943,
+      "pose" : {
+        "translation" : {
+          "x" : 4.844617660534176,
+          "y" : 2.8863579019021692
+        },
+        "rotation" : {
+          "radians" : -3.061184313833844
+        }
+      },
+      "curvature" : 0.07035398883055903
+    }, {
+      "time" : 0.7617731677016681,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.556674489449135E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.740723337056639,
+          "y" : 2.8775931182280945
+        },
+        "rotation" : {
+          "radians" : -3.0535882043682183
+        }
+      },
+      "curvature" : 0.07532598833602047
+    }, {
+      "time" : 0.7963729965952918,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.637362795500621,
+          "y" : 2.868057297106702
+        },
+        "rotation" : {
+          "radians" : -3.045519247910405
+        }
+      },
+      "curvature" : 0.08012479693513168
+    }, {
+      "time" : 0.8307945521766903,
+      "velocity" : 3.0,
+      "acceleration" : -8.652000136404901E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.534617028871959,
+          "y" : 2.857718107695623
+        },
+        "rotation" : {
+          "radians" : -3.0370030926303286
+        }
+      },
+      "curvature" : 0.08480128843727726
+    }, {
+      "time" : 0.8650131685157781,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 8.710165382862835E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 4.432570639269606,
+          "y" : 2.846548608755203
+        },
+        "rotation" : {
+          "radians" : -3.028061196597019
+        }
+      },
+      "curvature" : 0.08940649170847131
+    }, {
+      "time" : 0.899003277798418,
+      "velocity" : 3.0,
+      "acceleration" : -0.09574616288107782,
+      "pose" : {
+        "translation" : {
+          "x" : 4.331311419493517,
+          "y" : 2.834527037653644
+        },
+        "rotation" : {
+          "radians" : -3.018710894247568
+        }
+      },
+      "curvature" : 0.09399191846371814
+    }, {
+      "time" : 0.9327567117085453,
+      "velocity" : 2.9967682382190453,
+      "acceleration" : -0.16603740413403223,
+      "pose" : {
+        "translation" : {
+          "x" : 4.230929934652522,
+          "y" : 2.821636599372141
+        },
+        "rotation" : {
+          "radians" : -3.0089654537653083
+        }
+      },
+      "curvature" : 0.0986099907832933
+    }, {
+      "time" : 0.9662773051518138,
+      "velocity" : 2.9912025658986927,
+      "acceleration" : -0.17374271650320738,
+      "pose" : {
+        "translation" : {
+          "x" : 4.131519103772213,
+          "y" : 2.807865255510019
+        },
+        "rotation" : {
+          "radians" : -2.9988341203843856
+        }
+      },
+      "curvature" : 0.10331456682545719
+    }, {
+      "time" : 0.9995509183178184,
+      "velocity" : 2.985421517959354,
+      "acceleration" : -0.18358691510613726,
+      "pose" : {
+        "translation" : {
+          "x" : 4.033173781402816,
+          "y" : 2.793205513289873
+        },
+        "rotation" : {
+          "radians" : -2.9883221412125427
+        }
+      },
+      "curvature" : 0.10816156739888892
+    }, {
+      "time" : 1.0325512170508016,
+      "velocity" : 2.979363094917385,
+      "acceleration" : -0.1957881485667769,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9359903392270725,
+          "y" : 2.7776542145627077
+        },
+        "rotation" : {
+          "radians" : -2.977430767583505
+        }
+      },
+      "curvature" : 0.11320971018612586
+    }, {
+      "time" : 1.0652520605900002,
+      "velocity" : 2.9729606573042733,
+      "acceleration" : -0.2106132308438905,
+      "pose" : {
+        "translation" : {
+          "x" : 3.840066247668119,
+          "y" : 2.761212324813073
+        },
+        "rotation" : {
+          "radians" : -2.9661572312387867
+        }
+      },
+      "curvature" : 0.11852136257591563
+    }, {
+      "time" : 1.0976276482842051,
+      "velocity" : 2.966141930179527,
+      "acceleration" : -0.22838513501532773,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7454996574973665,
+          "y" : 2.743884722164204
+        },
+        "rotation" : {
+          "radians" : -2.9544946908190806
+        }
+      },
+      "curvature" : 0.1241635283632123
+    }, {
+      "time" : 1.1296526738305466,
+      "velocity" : 2.9588278903962566,
+      "acceleration" : -0.2494917017630424,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6523889814423782,
+          "y" : 2.7256799863831596
+        },
+        "rotation" : {
+          "radians" : -2.942432145241093
+        }
+      },
+      "curvature" : 0.1302089880914559
+    }, {
+      "time" : 1.1613024877721105,
+      "velocity" : 2.950931524455492,
+      "acceleration" : -0.27439562802342504,
+      "pose" : {
+        "translation" : {
+          "x" : 3.560832475794749,
+          "y" : 2.7066101878859605
+        },
+        "rotation" : {
+          "radians" : -2.9299543105725916
+        }
+      },
+      "curvature" : 0.13673761759648564
+    }, {
+      "time" : 1.1925532689948115,
+      "velocity" : 2.9423564467156664,
+      "acceleration" : -0.30364572614284985,
+      "pose" : {
+        "translation" : {
+          "x" : 3.470927822017984,
+          "y" : 2.686690676742728
+        },
+        "rotation" : {
+          "radians" : -2.9170414570270506
+        }
+      },
+      "curvature" : 0.1438379143723059
+    }, {
+      "time" : 1.2233822059663813,
+      "velocity" : 2.932995371762722,
+      "acceleration" : -0.3378893144869457,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3827717083553797,
+          "y" : 2.6659398716828218
+        },
+        "rotation" : {
+          "radians" : -2.9036692027182647
+        }
+      },
+      "curvature" : 0.15160876664672418
+    }, {
+      "time" : 1.253767688448565,
+      "velocity" : 2.922728441916462,
+      "acceleration" : -0.377885389511768,
+      "pose" : {
+        "translation" : {
+          "x" : 3.296459411437901,
+          "y" : 2.644379049099978
+        },
+        "rotation" : {
+          "radians" : -2.8898082608960753
+        }
+      },
+      "curvature" : 0.1601615053363712
+    }, {
+      "time" : 1.2836895103731156,
+      "velocity" : 2.9114214225836013,
+      "acceleration" : -0.4245178992880957,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2120843778920607,
+          "y" : 2.62203213205745
+        },
+        "rotation" : {
+          "radians" : -2.875424137597093
+        }
+      },
+      "curvature" : 0.1696222839550847
+    }, {
+      "time" : 1.313129084481963,
+      "velocity" : 2.898923796426977,
+      "acceleration" : -0.47880793939403443,
+      "pose" : {
+        "translation" : {
+          "x" : 3.129737805947798,
+          "y" : 2.5989254792931433
+        },
+        "rotation" : {
+          "radians" : -2.860476777086651
+        }
+      },
+      "curvature" : 0.180134835376471
+    }, {
+      "time" : 1.3420696691632106,
+      "velocity" : 2.8850668147108904,
+      "acceleration" : -0.5419229606175602,
+      "pose" : {
+        "translation" : {
+          "x" : 3.049508227046359,
+          "y" : 2.5750876742247573
+        },
+        "rotation" : {
+          "radians" : -2.844920153276735
+        }
+      },
+      "curvature" : 0.19186365592042148
+    }, {
+      "time" : 1.37049660762701,
+      "velocity" : 2.869661604057295,
+      "acceleration" : -0.6151800394568323,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9714810874481756,
+          "y" : 2.550549313954921
+        },
+        "rotation" : {
+          "radians" : -2.828701806670222
+        }
+      },
+      "curvature" : 0.2049976646319762
+    }, {
+      "time" : 1.3983975791037442,
+      "velocity" : 2.852497483323354,
+      "acceleration" : -0.7000388399543165,
+      "pose" : {
+        "translation" : {
+          "x" : 2.895738329840743,
+          "y" : 2.5253427982763332
+        },
+        "rotation" : {
+          "radians" : -2.8117623285706195
+        }
+      },
+      "curvature" : 0.21975437583412769
+    }, {
+      "time" : 1.4257628610363704,
+      "velocity" : 2.8333407231042154,
+      "acceleration" : -0.8320723157548291,
+      "pose" : {
+        "translation" : {
+          "x" : 2.822357974946499,
+          "y" : 2.499502118676901
+        },
+        "rotation" : {
+          "radians" : -2.7940347976772673
+        }
+      },
+      "curvature" : 0.23638460141375914
+    }, {
+      "time" : 1.4525899504690478,
+      "velocity" : 2.8110186446750056,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.751413703130705,
+          "y" : 2.473062647344876
+        },
+        "rotation" : {
+          "radians" : -2.7754441792642495
+        }
+      },
+      "curvature" : 0.25517765880922155
+    }, {
+      "time" : 1.4792695579561794,
+      "velocity" : 2.7043002147264787,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6829744360093244,
+          "y" : 2.4460609261739954
+        },
+        "rotation" : {
+          "radians" : -2.7559067046024177
+        }
+      },
+      "curvature" : 0.27646699088034715
+    }, {
+      "time" : 1.506205081648366,
+      "velocity" : 2.5965581199577317,
+      "acceleration" : -4.000000000000006,
+      "pose" : {
+        "translation" : {
+          "x" : 2.617103918056899,
+          "y" : 2.418534455768619
+        },
+        "rotation" : {
+          "radians" : -2.7353292590414324
+        }
+      },
+      "curvature" : 0.30063598962569904
+    }, {
+      "time" : 1.533414427222619,
+      "velocity" : 2.4877207376607204,
+      "acceleration" : -3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 2.553860298214431,
+          "y" : 2.3905214844488683
+        },
+        "rotation" : {
+          "radians" : -2.713608822413696
+        }
+      },
+      "curvature" : 0.32812363568576425
+    }, {
+      "time" : 1.5609222653091537,
+      "velocity" : 2.3776893853145813,
+      "acceleration" : -3.949364309865745,
+      "pose" : {
+        "translation" : {
+          "x" : 2.493295711497262,
+          "y" : 2.3620607972557637
+        },
+        "rotation" : {
+          "radians" : -2.690632026659688
+        }
+      },
+      "curvature" : 0.35942929054988465
+    }, {
+      "time" : 1.5887534138112722,
+      "velocity" : 2.267774040717741,
+      "acceleration" : -3.7932782770182274,
+      "pose" : {
+        "translation" : {
+          "x" : 2.43545586060295,
+          "y" : 2.333191504956364
+        },
+        "rotation" : {
+          "radians" : -2.6662749246206054
+        }
+      },
+      "curvature" : 0.3951155704482967
+    }, {
+      "time" : 1.6454161870589727,
+      "velocity" : 2.0528363738416293,
+      "acceleration" : -3.524370787870329,
+      "pose" : {
+        "translation" : {
+          "x" : 2.328098505131493,
+          "y" : 2.2743839107679378
+        },
+        "rotation" : {
+          "radians" : -2.612872321869423
+        }
+      },
+      "curvature" : 0.4821864866479269
+    }, {
+      "time" : 1.7034881688217829,
+      "velocity" : 1.8481691777230431,
+      "acceleration" : -3.1886854801463445,
+      "pose" : {
+        "translation" : {
+          "x" : 2.232009308124283,
+          "y" : 2.214410084736084
+        },
+        "rotation" : {
+          "radians" : -2.552217385705653
+        }
+      },
+      "curvature" : 0.5948949158111395
+    }, {
+      "time" : 1.7629801786134969,
+      "velocity" : 1.658467869915481,
+      "acceleration" : -2.793593360246646,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1472796517252846,
+          "y" : 2.153573117658766
+        },
+        "rotation" : {
+          "radians" : -2.4830442955743517
+        }
+      },
+      "curvature" : 0.7387705578516988
+    }, {
+      "time" : 1.823812432610913,
+      "velocity" : 1.4885272890594612,
+      "acceleration" : -2.4614261655202934,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0738578353503314,
+          "y" : 2.092161056443846
+        },
+        "rotation" : {
+          "radians" : -2.404174055212181
+        }
+      },
+      "curvature" : 0.9170860803614409
+    }, {
+      "time" : 1.8547141636761173,
+      "velocity" : 1.4124649596556962,
+      "acceleration" : -2.226093041455955,
+      "pose" : {
+        "translation" : {
+          "x" : 2.041328791717895,
+          "y" : 2.0613236050452572
+        },
+        "rotation" : {
+          "radians" : -2.3608351537787935
+        }
+      },
+      "curvature" : 1.0185171315348982
+    }, {
+      "time" : 1.8858592963049856,
+      "velocity" : 1.3431329966353498,
+      "acceleration" : -1.9794032204108298,
+      "pose" : {
+        "translation" : {
+          "x" : 2.011535687139258,
+          "y" : 2.0304401522735134
+        },
+        "rotation" : {
+          "radians" : -2.314846284252715
+        }
+      },
+      "curvature" : 1.1263819090275555
+    }, {
+      "time" : 1.9171879104709784,
+      "velocity" : 1.2811210368641754,
+      "acceleration" : -1.7210458088826468,
+      "pose" : {
+        "translation" : {
+          "x" : 1.98442585493639,
+          "y" : 1.9995394662212282
+        },
+        "rotation" : {
+          "radians" : -2.26626721249112
+        }
+      },
+      "curvature" : 1.2380648061508586
+    }, {
+      "time" : 1.9486228942778938,
+      "velocity" : 1.22701998973099,
+      "acceleration" : -1.4497718123734797,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9599351754080274,
+          "y" : 1.9686481087687149
+        },
+        "rotation" : {
+          "radians" : -2.2152625603384335
+        }
+      },
+      "curvature" : 1.3496477269860494
+    }, {
+      "time" : 1.9800679118911164,
+      "velocity" : 1.1814318895557523,
+      "acceleration" : -1.1629063029917064,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9379876574375503,
+          "y" : 1.9377902245891243
+        },
+        "rotation" : {
+          "radians" : -2.162125525962242
+        }
+      },
+      "curvature" : 1.4558154532790182
+    }, {
+      "time" : 2.0114057081537027,
+      "velocity" : 1.144988968760121,
+      "acceleration" : -0.8555700128978933,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9184950201008633,
+          "y" : 1.9069873301535836
+        },
+        "rotation" : {
+          "radians" : -2.1072976365024556
+        }
+      },
+      "curvature" : 1.5499621702421391
+    }, {
+      "time" : 2.042496890239119,
+      "velocity" : 1.1183882857022904,
+      "acceleration" : -0.5193738693932407,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9013562742742751,
+          "y" : 1.8762581027363336
+        },
+        "rotation" : {
+          "radians" : -2.051380200524095
+        }
+      },
+      "curvature" : 1.6245702075644999
+    }, {
+      "time" : 2.0731792244008695,
+      "velocity" : 1.1024526830866856,
+      "acceleration" : -0.1401133909627551,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8864573042423771,
+          "y" : 1.8456181694198683
+        },
+        "rotation" : {
+          "radians" : -1.9951332453181105
+        }
+      },
+      "curvature" : 1.6718749322032591
+    }, {
+      "time" : 2.103267289985402,
+      "velocity" : 1.098236942190127,
+      "acceleration" : 0.3065421531438961,
+      "pose" : {
+        "translation" : {
+          "x" : 1.873670449305921,
+          "y" : 1.8150798961000731
+        },
+        "rotation" : {
+          "radians" : -1.939459195747904
+        }
+      },
+      "curvature" : 1.6847350337062899
+    }, {
+      "time" : 2.132552032689131,
+      "velocity" : 1.1072139502727931,
+      "acceleration" : 0.8636627394205474,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8628540853896993,
+          "y" : 1.7846521764913632
+        },
+        "rotation" : {
+          "radians" : -1.885371407123151
+        }
+      },
+      "curvature" : 1.6575269774282888
+    }, {
+      "time" : 2.1607993444095768,
+      "velocity" : 1.1316101008945396,
+      "acceleration" : 1.6117144791019087,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8538522066504246,
+          "y" : 1.7543402211318213
+        },
+        "rotation" : {
+          "radians" : -1.8339512599110672
+        }
+      },
+      "curvature" : 1.5868287956600426
+    }, {
+      "time" : 2.1877462109747206,
+      "velocity" : 1.175040755904009,
+      "acceleration" : 3.6208105431392568,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8464940070846083,
+          "y" : 1.7241453463883367
+        },
+        "rotation" : {
+          "radians" : -1.7863006275868372
+        }
+      },
+      "curvature" : 1.4716950974629326
+    }, {
+      "time" : 2.236042382237465,
+      "velocity" : 1.349912042005413,
+      "acceleration" : 3.9999999999999813,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8359489103056603,
+          "y" : 1.6640913673919613
+        },
+        "rotation" : {
+          "radians" : -1.7065641844274897
+        }
+      },
+      "curvature" : 1.11509729054641
+    }, {
+      "time" : 2.277907500803531,
+      "velocity" : 1.517372516269675,
+      "acceleration" : 3.9833148393409696,
+      "pose" : {
+        "translation" : {
+          "x" : 1.829540444920319,
+          "y" : 1.6044148692087425
+        },
+        "rotation" : {
+          "radians" : -1.654002070934546
+        }
+      },
+      "curvature" : 0.613882276060416
+    }, {
+      "time" : 2.315345763310788,
+      "velocity" : 1.6665009028739752,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 1.825326681137085,
+          "y" : 1.5449646711349487
+        },
+        "rotation" : {
+          "radians" : -1.6352154187320553
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 2.3528636610794607,
+      "velocity" : 1.5164293117992842,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8219009519815899,
+          "y" : 1.485354601351787
+        },
+        "rotation" : {
+          "radians" : -1.6145927519610666
+        }
+      },
+      "curvature" : 0.6613132762682022
+    }, {
+      "time" : 2.3953009609475626,
+      "velocity" : 1.3466801123268766,
+      "acceleration" : -2.447765685928921,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8207798104572248,
+          "y" : 1.4246136307510824
+        },
+        "rotation" : {
+          "radians" : -1.559258462845717
+        }
+      },
+      "curvature" : 1.12045601019587
+    }, {
+      "time" : 2.4441469639679103,
+      "velocity" : 1.2271165422388892,
+      "acceleration" : -0.5720634881939421,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8239080891303416,
+          "y" : 1.361831680281739
+        },
+        "rotation" : {
+          "radians" : -1.480370605655411
+        }
+      },
+      "curvature" : 1.349435348223791
+    }, {
+      "time" : 2.4708676658243456,
+      "velocity" : 1.2118306043279066,
+      "acceleration" : 0.15979441326309798,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8275716032411027,
+          "y" : 1.3294530877098598
+        },
+        "rotation" : {
+          "radians" : -1.43570297822473
+        }
+      },
+      "curvature" : 1.3836934054080776
+    }, {
+      "time" : 2.498501305753145,
+      "velocity" : 1.2162463056066528,
+      "acceleration" : 0.7244111612153562,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8328603306872537,
+          "y" : 1.2963242800469743
+        },
+        "rotation" : {
+          "radians" : -1.389335893805699
+        }
+      },
+      "curvature" : 1.3736643754703268
+    }, {
+      "time" : 2.526759486377394,
+      "velocity" : 1.2367168470464982,
+      "acceleration" : 1.1926306446746133,
+      "pose" : {
+        "translation" : {
+          "x" : 1.839917232537418,
+          "y" : 1.2623921911031442
+        },
+        "rotation" : {
+          "radians" : -1.3424216946735859
+        }
+      },
+      "curvature" : 1.3285660480051897
+    }, {
+      "time" : 2.5553998339251613,
+      "velocity" : 1.270874203206097,
+      "acceleration" : 1.601933018243381,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8488651751439278,
+          "y" : 1.2276157532543266
+        },
+        "rotation" : {
+          "radians" : -1.2959209814340706
+        }
+      },
+      "curvature" : 1.258109886245163
+    }, {
+      "time" : 2.5842198818567117,
+      "velocity" : 1.317041989575004,
+      "acceleration" : 1.9730587889589195,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8598076922431273,
+          "y" : 1.191965371940812
+        },
+        "rotation" : {
+          "radians" : -1.2505887693826183
+        }
+      },
+      "curvature" : 1.171451900081923
+    }, {
+      "time" : 2.613052469764473,
+      "velocity" : 1.3739303805548424,
+      "acceleration" : 2.4837574247785343,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8728297470556754,
+          "y" : 1.1554224001656621
+        },
+        "rotation" : {
+          "radians" : -1.206979322113377
+        }
+      },
+      "curvature" : 1.0764509613846949
+    }, {
+      "time" : 2.670135599245487,
+      "velocity" : 1.5157110272329066,
+      "acceleration" : 3.0990658562394877,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9053640427268448,
+          "y" : 1.0796356820471829
+        },
+        "rotation" : {
+          "radians" : -1.1262654169222006
+        }
+      },
+      "curvature" : 0.8844858371105864
+    }, {
+      "time" : 2.7259954044656856,
+      "velocity" : 1.6888242423270121,
+      "acceleration" : 3.649016814110955,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9468053174205124,
+          "y" : 1.000305405119434
+        },
+        "rotation" : {
+          "radians" : -1.0550981178089267
+        }
+      },
+      "curvature" : 0.7124506625993454
+    }, {
+      "time" : 2.7801731094475235,
+      "velocity" : 1.8865195987556824,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9972424725681748,
+          "y" : 0.9176229045073114
+        },
+        "rotation" : {
+          "radians" : -0.9932815877369497
+        }
+      },
+      "curvature" : 0.5709539223900668
+    }, {
+      "time" : 2.8325182827204705,
+      "velocity" : 2.0959002918474696,
+      "acceleration" : 3.999999999999984,
+      "pose" : {
+        "translation" : {
+          "x" : 2.05654044297944,
+          "y" : 0.8319042281900693
+        },
+        "rotation" : {
+          "radians" : -0.939869543584731
+        }
+      },
+      "curvature" : 0.45987023815540634
+    }, {
+      "time" : 2.8832022831916913,
+      "velocity" : 2.298636293732353,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 2.124364584051719,
+          "y" : 0.7435733209513273
+        },
+        "rotation" : {
+          "radians" : -0.8936544133402136
+        }
+      },
+      "curvature" : 0.37479587351455695
+    }, {
+      "time" : 2.9324371813867773,
+      "velocity" : 2.4955758865126962,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2002050589799182,
+          "y" : 0.6531452083290787
+        },
+        "rotation" : {
+          "radians" : -0.853427174289811
+        }
+      },
+      "curvature" : 0.3104667355759126
+    }, {
+      "time" : 2.980286705128464,
+      "velocity" : 2.686973981479444,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2834012259661307,
+          "y" : 0.5612091805656974
+        },
+        "rotation" : {
+          "radians" : -0.8180911393767909
+        }
+      },
+      "curvature" : 0.26216926405981567
+    }, {
+      "time" : 3.0036886028665157,
+      "velocity" : 2.7805815724316503,
+      "acceleration" : 1.7834018615548795,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3275160092188085,
+          "y" : 0.514876020137125
+        },
+        "rotation" : {
+          "radians" : -0.8019547141012324
+        }
+      },
+      "curvature" : 0.24281777874977586
+    }, {
+      "time" : 3.026940898356585,
+      "velocity" : 2.822049759494064,
+      "acceleration" : 0.6908039852530397,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3731660254293274,
+          "y" : 0.4684119765579453
+        },
+        "rotation" : {
+          "radians" : -0.7866964853329266
+        }
+      },
+      "curvature" : 0.22613732914460744
+    }, {
+      "time" : 3.050322026787958,
+      "velocity" : 2.8382015361941697,
+      "acceleration" : 0.5944336707391082,
+      "pose" : {
+        "translation" : {
+          "x" : 2.420237140398036,
+          "y" : 0.4219041096592999
+        },
+        "rotation" : {
+          "radians" : -0.772220104481907
+        }
+      },
+      "curvature" : 0.2118009615897061
+    }, {
+      "time" : 3.0738960629383696,
+      "velocity" : 2.8522147370371953,
+      "acceleration" : 0.5085625308709665,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4686103672150495,
+          "y" : 0.37544096780698055
+        },
+        "rotation" : {
+          "radians" : -0.7584370845254595
+        }
+      },
+      "curvature" : 0.19952992215767287
+    }, {
+      "time" : 3.097629651885521,
+      "velocity" : 2.86428475109881,
+      "acceleration" : 0.4316967127192361,
+      "pose" : {
+        "translation" : {
+          "x" : 2.518162628360552,
+          "y" : 0.3291120623998669
+        },
+        "rotation" : {
+          "radians" : -0.7452660870351325
+        }
+      },
+      "curvature" : 0.18908811609138015
+    }, {
+      "time" : 3.1214873211538543,
+      "velocity" : 2.874584028495092,
+      "acceleration" : 0.3624475659523509,
+      "pose" : {
+        "translation" : {
+          "x" : 2.5687675178050995,
+          "y" : 0.28300734236836433
+        },
+        "rotation" : {
+          "radians" : -0.7326322058073411
+        }
+      },
+      "curvature" : 0.1802768862425938
+    }, {
+      "time" : 3.145431891692516,
+      "velocity" : 2.8832626798046044,
+      "acceleration" : 0.2995436811273533,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6202960631099237,
+          "y" : 0.23721666867284164
+        },
+        "rotation" : {
+          "radians" : -0.7204662745977262
+        }
+      },
+      "curvature" : 0.17293027048351933
+    }, {
+      "time" : 3.1694248840625208,
+      "velocity" : 2.890449629060376,
+      "acceleration" : 0.24183189107987368,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6726174875272335,
+          "y" : 0.19182928880206873
+        },
+        "rotation" : {
+          "radians" : -0.708704216367118
+        }
+      },
+      "curvature" : 0.16691079279926538
+    }, {
+      "time" : 3.193426917481795,
+      "velocity" : 2.8962540861919215,
+      "acceleration" : 0.18827218906255389,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7255999721005186,
+          "y" : 0.14693331127165443
+        },
+        "rotation" : {
+          "radians" : -0.6972864449360306
+        }
+      },
+      "curvature" : 0.16210577857562763
+    }, {
+      "time" : 3.217398100152016,
+      "velocity" : 2.9007671932276624,
+      "acceleration" : 0.13793017248481926,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7791114177648524,
+          "y" : 0.10261518012248416
+        },
+        "rotation" : {
+          "radians" : -0.6861573260641398
+        }
+      },
+      "curvature" : 0.15842414582671857
+    }, {
+      "time" : 3.241298409734759,
+      "velocity" : 2.9040637670508507,
+      "acceleration" : 0.08996972588049867,
+      "pose" : {
+        "translation" : {
+          "x" : 2.8330202074471944,
+          "y" : 0.0589591494191577
+        },
+        "rotation" : {
+          "radians" : -0.6752647030503031
+        }
+      },
+      "curvature" : 0.1557936000148212
+    }, {
+      "time" : 3.2650880630305976,
+      "velocity" : 2.9062041156366694,
+      "acceleration" : 0.04364814484804298,
+      "pose" : {
+        "translation" : {
+          "x" : 2.8871959681666937,
+          "y" : 0.0160467577484269
+        },
+        "rotation" : {
+          "radians" : -0.6645594915329975
+        }
+      },
+      "curvature" : 0.15415814328341032
+    }, {
+      "time" : 3.288727873894095,
+      "velocity" : 2.9072359495254196,
+      "acceleration" : -0.0016842852381663946,
+      "pose" : {
+        "translation" : {
+          "x" : 2.941510333134991,
+          "y" : -0.02604369728236655
+        },
+        "rotation" : {
+          "radians" : -0.6539953489714027
+        }
+      },
+      "curvature" : 0.15347579410401332
+    }, {
+      "time" : 3.3121795982222406,
+      "velocity" : 2.907196450132324,
+      "acceleration" : -0.046578151707487234,
+      "pose" : {
+        "translation" : {
+          "x" : 2.995837703856523,
+          "y" : -0.06723768454685342
+        },
+        "rotation" : {
+          "radians" : -0.6435284261390796
+        }
+      },
+      "curvature" : 0.15371639662570263
+    }, {
+      "time" : 3.335406264503977,
+      "velocity" : 2.9061145949465943,
+      "acceleration" : -0.09146977902103462,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0500560122288234,
+          "y" : -0.10746496490119917
+        },
+        "rotation" : {
+          "radians" : -0.6331172107927046
+        }
+      },
+      "curvature" : 0.15485937730919708
+    }, {
+      "time" : 3.358372487917268,
+      "velocity" : 2.904013879566033,
+      "acceleration" : -0.13665410312300072,
+      "pose" : {
+        "translation" : {
+          "x" : 3.1040474826428266,
+          "y" : -0.14666011668566625
+        },
+        "rotation" : {
+          "radians" : -0.6227224774786422
+        }
+      },
+      "curvature" : 0.15689127710744225
+    }, {
+      "time" : 3.381044765324835,
+      "velocity" : 2.900915619831146,
+      "acceleration" : -2.1491915946775815,
+      "pose" : {
+        "translation" : {
+          "x" : 3.157699394083171,
+          "y" : -0.18476306122617636
+        },
+        "rotation" : {
+          "radians" : -0.6123073622363067
+        }
+      },
+      "curvature" : 0.15980284830484542
+    }, {
+      "time" : 3.403563912604914,
+      "velocity" : 2.8525176577774944,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2109048422285014,
+          "y" : -0.22171958833587269
+        },
+        "rotation" : {
+          "radians" : -0.6018375867750714
+        }
+      },
+      "curvature" : 0.16358545453108617
+    }, {
+      "time" : 3.4492280218552347,
+      "velocity" : 2.6698612207762125,
+      "acceleration" : -3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 3.315582387420548,
+          "y" : -0.2920090449608779
+        },
+        "rotation" : {
+          "radians" : -0.5806125205758887
+        }
+      },
+      "curvature" : 0.1737031415493062
+    }, {
+      "time" : 3.4961580129617453,
+      "velocity" : 2.48214125635017,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4173701773397624,
+          "y" : -0.35723214386962354
+        },
+        "rotation" : {
+          "radians" : -0.558846041472928
+        }
+      },
+      "curvature" : 0.18697261071178092
+    }, {
+      "time" : 3.544441517737483,
+      "velocity" : 2.2890072372472194,
+      "acceleration" : -4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 3.5157001126304266,
+          "y" : -0.4172200702665805
+        },
+        "rotation" : {
+          "radians" : -0.5364315370742367
+        }
+      },
+      "curvature" : 0.20264494363905447
+    }, {
+      "time" : 3.594311089348581,
+      "velocity" : 2.0895289508028285,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.610170322670001,
+          "y" : -0.471948353301741
+        },
+        "rotation" : {
+          "radians" : -0.5134162602944821
+        }
+      },
+      "curvature" : 0.2190045005586516
+    }, {
+      "time" : 3.6462406863199543,
+      "velocity" : 1.8818105629173347,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7005695527788163,
+          "y" : -0.5215536821206115
+        },
+        "rotation" : {
+          "radians" : -0.4900919671398013
+        }
+      },
+      "curvature" : 0.23265913487493842
+    }, {
+      "time" : 3.701128109083174,
+      "velocity" : 1.6622608718644565,
+      "acceleration" : -3.999999999999998,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7869015514297644,
+          "y" : -0.5663507219142048
+        },
+        "rotation" : {
+          "radians" : -0.4671173179465116
+        }
+      },
+      "curvature" : 0.23773186828932136
+    }, {
+      "time" : 3.7606892883012333,
+      "velocity" : 1.4240161549922183,
+      "acceleration" : -4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8694094574579907,
+          "y" : -0.6068489299690327
+        },
+        "rotation" : {
+          "radians" : -0.4456529747120997
+        }
+      },
+      "curvature" : 0.2254664901452733
+    }, {
+      "time" : 3.8285064753472717,
+      "velocity" : 1.1527474068080645,
+      "acceleration" : -3.9999999999999987,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9486001872705856,
+          "y" : -0.6437693717170987
+        },
+        "rotation" : {
+          "radians" : -0.42745091338906643
+        }
+      },
+      "curvature" : 0.1853952813106193
+    }, {
+      "time" : 3.9140667774226117,
+      "velocity" : 0.8105061985067056,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.025268822056276,
+          "y" : -0.67806153678589
+        },
+        "rotation" : {
+          "radians" : -0.414785534453574
+        }
+      },
+      "curvature" : 0.10957339292803865
+    }, {
+      "time" : 4.116693327049288,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.100522994995117,
+          "y" : -0.7109201550483704
+        },
+        "rotation" : {
+          "radians" : -0.41008727651812055
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 5.8985185623168945, -6.7758471965789795, 0.0 ],
+      "y" : [ 2.9429874420166016, -0.26568102836608887, 0.0 ]
+    }, {
+      "x" : [ 1.825326681137085, -0.12263345718383789, 0.0 ],
+      "y" : [ 1.5449646711349487, -1.9010478258132935, 0.0 ]
+    }, {
+      "x" : [ 4.100522994995117, 2.3999733924865723, 0.0 ],
+      "y" : [ -0.7109201550483704, -1.0433523654937744, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : -3.1364759703203093
+      }
+    }, {
+      "time" : 2.3099999483674765,
+      "rotation" : {
+        "radians" : 0.022477488801708422
+      }
+    }, {
+      "time" : 4.029999909922481,
+      "rotation" : {
+        "radians" : -0.44615049906220317
+      }
+    } ],
+    "reversed" : false,
+    "color" : 52.47058,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  }, {
+    "type" : "script",
+    "script" : "@t 2\nHopper.setHopperState REVERSE\nIntake.setWantedIntakeState EJECT",
+    "closed" : true,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 2.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Hopper.setHopperState",
+        "args" : [ "REVERSE" ],
+        "argTypes" : [ "frc.subsystem.Hopper$HopperState" ],
+        "reflection" : true
+      }, {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "EJECT" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "script",
+    "script" : "@t 3\nIntake.setWantedIntakeState OFF",
+    "closed" : false,
+    "valid" : true,
+    "sendableScript" : {
+      "delayType" : "TIME",
+      "delay" : 3.0,
+      "commands" : [ {
+        "methodName" : "frc.subsystem.Intake.setWantedIntakeState",
+        "args" : [ "OFF" ],
+        "argTypes" : [ "frc.subsystem.Intake$IntakeState" ],
+        "reflection" : true
+      } ]
+    }
+  }, {
+    "type" : "trajectory",
+    "states" : [ {
+      "time" : 0.0,
+      "velocity" : 0.0,
+      "acceleration" : 4.000000000000001,
+      "pose" : {
+        "translation" : {
+          "x" : 4.100522994995117,
+          "y" : -0.7109201550483704
+        },
+        "rotation" : {
+          "radians" : 2.5857927494599444
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 0.17905554113279049,
+      "velocity" : 0.716222164531162,
+      "acceleration" : 3.999999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 4.046051351866051,
+          "y" : -0.6770905534088945
+        },
+        "rotation" : {
+          "radians" : 2.5859290584771175
+        }
+      },
+      "curvature" : 0.004100780417876861
+    }, {
+      "time" : 0.2532222147861238,
+      "velocity" : 1.0128888591444951,
+      "acceleration" : 3.9999999999999982,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9915714570599867,
+          "y" : -0.6432753174175616
+        },
+        "rotation" : {
+          "radians" : 2.5862997183209484
+        }
+      },
+      "curvature" : 0.007316179708346309
+    }, {
+      "time" : 0.3101283388167845,
+      "velocity" : 1.2405133552671377,
+      "acceleration" : 4.000000000000002,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9370801190687295,
+          "y" : -0.6094880375241134
+        },
+        "rotation" : {
+          "radians" : 2.5868491896599624
+        }
+      },
+      "curvature" : 0.009686266454747228
+    }, {
+      "time" : 0.35809346841854367,
+      "velocity" : 1.4323738736741745,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 3.8825794348939553,
+          "y" : -0.5757414756226353
+        },
+        "rotation" : {
+          "radians" : 2.5875244773924204
+        }
+      },
+      "curvature" : 0.011252124346097734
+    }, {
+      "time" : 0.4003365631482469,
+      "velocity" : 1.6013462525929871,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.828076535205502,
+          "y" : -0.5420475838784169
+        },
+        "rotation" : {
+          "radians" : 2.588275148867047
+        }
+      },
+      "curvature" : 0.012054310762550804
+    }, {
+      "time" : 0.43850530180719144,
+      "velocity" : 1.7540212072287653,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 3.7735833294996617,
+          "y" : -0.5084175235548116
+        },
+        "rotation" : {
+          "radians" : 2.5890532903104244
+        }
+      },
+      "curvature" : 0.0121315702156112
+    }, {
+      "time" : 0.4735755889182925,
+      "velocity" : 1.8943023556731697,
+      "acceleration" : 3.9999999999999964,
+      "pose" : {
+        "translation" : {
+          "x" : 3.719116251257474,
+          "y" : -0.4748616838400972
+        },
+        "rotation" : {
+          "radians" : 2.589813411890699
+        }
+      },
+      "curvature" : 0.01151977337607476
+    }, {
+      "time" : 0.5061806573270884,
+      "velocity" : 2.024722629308353,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6646960031030176,
+          "y" : -0.4413897006743355
+        },
+        "rotation" : {
+          "radians" : 2.590512310722669
+        }
+      },
+      "curvature" : 0.010251050076436605
+    }, {
+      "time" : 0.5367579667986577,
+      "velocity" : 2.147031867194631,
+      "acceleration" : 4.000000000000007,
+      "pose" : {
+        "translation" : {
+          "x" : 3.610347301961702,
+          "y" : -0.4080104755762326
+        },
+        "rotation" : {
+          "radians" : 2.591108899964241
+        }
+      },
+      "curvature" : 0.008353084081511222
+    }, {
+      "time" : 0.5656238557211262,
+      "velocity" : 2.2624954228845047,
+      "acceleration" : 3.9999999999999893,
+      "pose" : {
+        "translation" : {
+          "x" : 3.55609862421856,
+          "y" : -0.37473219446999906
+        },
+        "rotation" : {
+          "radians" : 2.5915640110153895
+        }
+      },
+      "curvature" : 0.0058485380718137905
+    }, {
+      "time" : 0.6191140364610012,
+      "velocity" : 2.476456145844004,
+      "acceleration" : 4.0000000000000036,
+      "pose" : {
+        "translation" : {
+          "x" : 3.4480325127148035,
+          "y" : -0.308507742918664
+        },
+        "rotation" : {
+          "radians" : 2.5919013866336735
+        }
+      },
+      "curvature" : -9.17525897781337E-4
+    }, {
+      "time" : 0.6679788967836321,
+      "velocity" : 2.671915587134528,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.340790984879593,
+          "y" : -0.24276823694478367
+        },
+        "rotation" : {
+          "radians" : 2.591240769502497
+        }
+      },
+      "curvature" : -0.009982710275132317
+    }, {
+      "time" : 0.7130614555741249,
+      "velocity" : 2.8522458222964993,
+      "acceleration" : 3.5172385867566596,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2347111984854564,
+          "y" : -0.17755532287992537
+        },
+        "rotation" : {
+          "radians" : 2.5893138286617203
+        }
+      },
+      "curvature" : -0.02138799605984592
+    }, {
+      "time" : 0.7550700296469314,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 7.342027890777618E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.130165997854956,
+          "y" : -0.11290100492229271
+        },
+        "rotation" : {
+          "radians" : 2.5858591271774563
+        }
+      },
+      "curvature" : -0.035290971114166826
+    }, {
+      "time" : 0.7953939670709853,
+      "velocity" : 3.0,
+      "acceleration" : -7.485000429419822E-15,
+      "pose" : {
+        "translation" : {
+          "x" : 3.0275557589260416,
+          "y" : -0.04882824759624782
+        },
+        "rotation" : {
+          "radians" : 2.5806134504141105
+        }
+      },
+      "curvature" : -0.05197909891742402
+    }, {
+      "time" : 0.8349476686405554,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : -0.5537636550923151,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9273002343174035,
+          "y" : 0.014648421788166388
+        },
+        "rotation" : {
+          "radians" : 2.573303027054985
+        }
+      },
+      "curvature" : -0.07188677132820555
+    }, {
+      "time" : 0.87374969323478,
+      "velocity" : 2.97851284903572,
+      "acceleration" : -0.9070822146479386,
+      "pose" : {
+        "translation" : {
+          "x" : 2.829830398393824,
+          "y" : 0.07752231067570392
+        },
+        "rotation" : {
+          "radians" : 2.563634738472427
+        }
+      },
+      "curvature" : -0.095617170645365
+    }, {
+      "time" : 0.9118975599042641,
+      "velocity" : 2.94390959765307,
+      "acceleration" : -2.262555771483939,
+      "pose" : {
+        "translation" : {
+          "x" : 2.73558029233153,
+          "y" : 0.13979395880639345
+        },
+        "rotation" : {
+          "radians" : 2.5512874700147776
+        }
+      },
+      "curvature" : -0.12396891770742956
+    }, {
+      "time" : 0.9496761220339536,
+      "velocity" : 2.8584334938681764,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6449788691835465,
+          "y" : 0.20147053575601603
+        },
+        "rotation" : {
+          "radians" : 2.5359039299223154
+        }
+      },
+      "curvature" : -0.15796560005178947
+    }, {
+      "time" : 0.9877492069346551,
+      "velocity" : 2.7061411542653704,
+      "acceleration" : -3.9999999999999956,
+      "pose" : {
+        "translation" : {
+          "x" : 2.558441838945047,
+          "y" : 0.26256523847658286
+        },
+        "rotation" : {
+          "radians" : 2.517083600091246
+        }
+      },
+      "curvature" : -0.19888275461263835
+    }, {
+      "time" : 1.0265482053771444,
+      "velocity" : 2.550945160495413,
+      "acceleration" : -4.000000000000004,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4763635136187077,
+          "y" : 0.323096688836813
+        },
+        "rotation" : {
+          "radians" : 2.494378068434297
+        }
+      },
+      "curvature" : -0.2482604413289035
+    }, {
+      "time" : 1.0661195848558005,
+      "velocity" : 2.3926596425807887,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3991086522800593,
+          "y" : 0.383088331162611
+        },
+        "rotation" : {
+          "radians" : 2.4672909353168477
+        }
+      },
+      "curvature" : -0.3078783541033658
+    }, {
+      "time" : 1.1065518464830886,
+      "velocity" : 2.230930596071637,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 2.3270043061428396,
+          "y" : 0.44256782977754483
+        },
+        "rotation" : {
+          "radians" : 2.4352859073052295
+        }
+      },
+      "curvature" : -0.37965241689117946
+    }, {
+      "time" : 1.1479982964991189,
+      "velocity" : 2.065144796007515,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2603316636243456,
+          "y" : 0.5015664665433235
+        },
+        "rotation" : {
+          "radians" : 2.397808651299899
+        }
+      },
+      "curvature" : -0.46538626343634765
+    }, {
+      "time" : 1.1907134105418258,
+      "velocity" : 1.8942843398366878,
+      "acceleration" : -3.8004076554878155,
+      "pose" : {
+        "translation" : {
+          "x" : 2.199317895410786,
+          "y" : 0.5601185384002747
+        },
+        "rotation" : {
+          "radians" : 2.35433030289942
+        }
+      },
+      "curvature" : -0.5662827934211753
+    }, {
+      "time" : 1.2350003317172529,
+      "velocity" : 1.725975985563609,
+      "acceleration" : -3.096610871868397,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1441279995226346,
+          "y" : 0.6182607549078227
+        },
+        "rotation" : {
+          "radians" : 2.304422426440884
+        }
+      },
+      "curvature" : -0.6821096692467903
+    }, {
+      "time" : 1.2808802623624265,
+      "velocity" : 1.5839036935271966,
+      "acceleration" : -2.455893226230777,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0948566463799807,
+          "y" : 0.6760316357849661
+        },
+        "rotation" : {
+          "radians" : 2.24787278990788
+        }
+      },
+      "curvature" : -0.8099648281546794
+    }, {
+      "time" : 1.3280319668435383,
+      "velocity" : 1.4681041418867986,
+      "acceleration" : -1.8499614404769908,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0515200238678837,
+          "y" : 0.7334709084507551
+        },
+        "rotation" : {
+          "radians" : 2.184845091649205
+        }
+      },
+      "curvature" : -0.9427791639774807
+    }, {
+      "time" : 1.376032015282584,
+      "velocity" : 1.3793059031335362,
+      "acceleration" : -1.248435182006515,
+      "pose" : {
+        "translation" : {
+          "x" : 2.014047682401724,
+          "y" : 0.7906189055647701
+        },
+        "rotation" : {
+          "radians" : 2.1160696333836517
+        }
+      },
+      "curvature" : -1.0680768788914654
+    }, {
+      "time" : 1.424334569704738,
+      "velocity" : 1.3190032948121349,
+      "acceleration" : -0.6091506293120736,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9822743799925568,
+          "y" : 0.8475159625675985
+        },
+        "rotation" : {
+          "radians" : 2.0430250430635404
+        }
+      },
+      "curvature" : -1.1679706858124066
+    }, {
+      "time" : 1.4722549198410007,
+      "velocity" : 1.2898125833697756,
+      "acceleration" : 0.14219135021541737,
+      "pose" : {
+        "translation" : {
+          "x" : 1.955931927312463,
+          "y" : 0.9042018152213132
+        },
+        "rotation" : {
+          "radians" : 1.9680433024899477
+        }
+      },
+      "curvature" : -1.221435348671096
+    }, {
+      "time" : 1.5189560337976418,
+      "velocity" : 1.2964530778198344,
+      "acceleration" : 1.1656913546554033,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9346410327599024,
+          "y" : 0.9607149971499496
+        },
+        "rotation" : {
+          "radians" : 1.894265907226921
+        }
+      },
+      "curvature" : -1.2089548933496592
+    }, {
+      "time" : 1.563428663287173,
+      "velocity" : 1.348294437534574,
+      "acceleration" : 2.8841346574602866,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9179031475250667,
+          "y" : 1.017092237379984
+        },
+        "rotation" : {
+          "radians" : 1.8254232181396424
+        }
+      },
+      "curvature" : -1.1177745520566962
+    }, {
+      "time" : 1.604436283065203,
+      "velocity" : 1.4665659349563442,
+      "acceleration" : 3.999999999999992,
+      "pose" : {
+        "translation" : {
+          "x" : 1.905092310655231,
+          "y" : 1.073367857880811
+        },
+        "rotation" : {
+          "radians" : 1.7654947807008987
+        }
+      },
+      "curvature" : -0.9447578680023896
+    }, {
+      "time" : 1.6414523727107633,
+      "velocity" : 1.6146302935385846,
+      "acceleration" : 3.7005826612074797,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8954469941201069,
+          "y" : 1.1295731711052213
+        },
+        "rotation" : {
+          "radians" : 1.7183797533566942
+        }
+      },
+      "curvature" : -0.6949978934363836
+    }, {
+      "time" : 1.706643337288398,
+      "velocity" : 1.855874846721971,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8818800449371338,
+          "y" : 1.2418794631958008
+        },
+        "rotation" : {
+          "radians" : 1.676778825261928
+        }
+      },
+      "curvature" : -0.0
+    }, {
+      "time" : 1.7723136975830924,
+      "velocity" : 1.5931934055431933,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8715964211490927,
+          "y" : 1.3546623770804445
+        },
+        "rotation" : {
+          "radians" : 1.632957005658238
+        }
+      },
+      "curvature" : -0.7254972162927384
+    }, {
+      "time" : 1.810034487394077,
+      "velocity" : 1.4423102462992545,
+      "acceleration" : -2.710726662081756,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8693755361295565,
+          "y" : 1.4118700819531647
+        },
+        "rotation" : {
+          "radians" : 1.5838413049635738
+        }
+      },
+      "curvature" : -0.9768015183979825
+    }, {
+      "time" : 1.8519046258057839,
+      "velocity" : 1.3288117457615873,
+      "acceleration" : -1.1894816130443073,
+      "pose" : {
+        "translation" : {
+          "x" : 1.870371506531228,
+          "y" : 1.4698751626601734
+        },
+        "rotation" : {
+          "radians" : 1.521732134311164
+        }
+      },
+      "curvature" : -1.1507918750528034
+    }, {
+      "time" : 1.8972897575518486,
+      "velocity" : 1.2748269660440499,
+      "acceleration" : -0.3477506556011637,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8753421949347988,
+          "y" : 1.528748941692104
+        },
+        "rotation" : {
+          "radians" : 1.4504225267826032
+        }
+      },
+      "curvature" : -1.2503201227521346
+    }, {
+      "time" : 1.9450499018565859,
+      "velocity" : 1.2582183445504713,
+      "acceleration" : 0.2016282832037308,
+      "pose" : {
+        "translation" : {
+          "x" : 1.8849010019640673,
+          "y" : 1.5884782075607973
+        },
+        "rotation" : {
+          "radians" : 1.3734581625763413
+        }
+      },
+      "curvature" : -1.28354671036822
+    }, {
+      "time" : 1.9943208439648752,
+      "velocity" : 1.268152760019596,
+      "acceleration" : 0.5969274636685241,
+      "pose" : {
+        "translation" : {
+          "x" : 1.89952602502051,
+          "y" : 1.6489738267635623
+        },
+        "rotation" : {
+          "radians" : 1.293941372408038
+        }
+      },
+      "curvature" : -1.2635154627438518
+    }, {
+      "time" : 2.0444402181362453,
+      "velocity" : 1.2980703909243658,
+      "acceleration" : 0.8935195397370149,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9195692170178518,
+          "y" : 1.710079355747439
+        },
+        "rotation" : {
+          "radians" : 1.2143690380279577
+        }
+      },
+      "curvature" : -1.2059442083489496
+    }, {
+      "time" : 2.094911061001386,
+      "velocity" : 1.3431670752113656,
+      "acceleration" : 1.1141266498975333,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9452655451166372,
+          "y" : 1.7715796528734593
+        },
+        "rotation" : {
+          "radians" : 1.1365566560794031
+        }
+      },
+      "curvature" : -1.1263247530634717
+    }, {
+      "time" : 2.1453768398619966,
+      "velocity" : 1.3993923443478076,
+      "acceleration" : 1.2676595640303998,
+      "pose" : {
+        "translation" : {
+          "x" : 1.9767421494588007,
+          "y" : 1.8332094903809093
+        },
+        "rotation" : {
+          "radians" : 1.061661442713639
+        }
+      },
+      "curvature" : -1.0376352482876874
+    }, {
+      "time" : 2.1955990497229334,
+      "velocity" : 1.4630570090047659,
+      "acceleration" : 1.3576641534621845,
+      "pose" : {
+        "translation" : {
+          "x" : 2.0140275019022376,
+          "y" : 1.8946621663515906
+        },
+        "rotation" : {
+          "radians" : 0.9902801108323195
+        }
+      },
+      "curvature" : -0.949295026502291
+    }, {
+      "time" : 2.2454352355231433,
+      "velocity" : 1.5307178120109917,
+      "acceleration" : 1.3867285924070238,
+      "pose" : {
+        "translation" : {
+          "x" : 2.057060564755375,
+          "y" : 1.9555981166740821
+        },
+        "rotation" : {
+          "radians" : 0.9225815953395504
+        }
+      },
+      "curvature" : -0.8672282806254888
+    }, {
+      "time" : 2.2948177631789672,
+      "velocity" : 1.5991979750766532,
+      "acceleration" : 1.3589820685583802,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1056999495117417,
+          "y" : 2.015653527008002
+        },
+        "rotation" : {
+          "radians" : 0.858440269383883
+        }
+      },
+      "curvature" : -0.7945463578337496
+    }, {
+      "time" : 2.3437341392878004,
+      "velocity" : 1.6656744530674152,
+      "acceleration" : 1.2814168669463941,
+      "pose" : {
+        "translation" : {
+          "x" : 2.1597330755845405,
+          "y" : 2.0744489447482692
+        },
+        "rotation" : {
+          "radians" : 0.7975494729913294
+        }
+      },
+      "curvature" : -0.7323917673147309
+    }, {
+      "time" : 2.3922093962778366,
+      "velocity" : 1.7277914650040085,
+      "acceleration" : 1.1642994999905252,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2188853290412176,
+          "y" : 2.131597890989365
+        },
+        "rotation" : {
+          "radians" : 0.7395085520599514
+        }
+      },
+      "curvature" : -0.6806769668594456
+    }, {
+      "time" : 2.4402906479447277,
+      "velocity" : 1.7837724422786883,
+      "acceleration" : 1.0208386073494335,
+      "pose" : {
+        "translation" : {
+          "x" : 2.2828292213380337,
+          "y" : 2.1867154724895954
+        },
+        "rotation" : {
+          "radians" : 0.6838850476440103
+        }
+      },
+      "curvature" : -0.6386233642001887
+    }, {
+      "time" : 2.4880336196518704,
+      "velocity" : 1.8325103110269312,
+      "acceleration" : 0.8663296664660994,
+      "pose" : {
+        "translation" : {
+          "x" : 2.351193548054635,
+          "y" : 2.2394269936353517
+        },
+        "rotation" : {
+          "radians" : 0.6302573876108508
+        }
+      },
+      "curvature" : -0.6051051506550083
+    }, {
+      "time" : 2.5354908013854165,
+      "velocity" : 1.873623875449575,
+      "acceleration" : 0.7170825323129972,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4235725476286234,
+          "y" : 2.2893765684053733
+        },
+        "rotation" : {
+          "radians" : 0.5782441181597022
+        }
+      },
+      "curvature" : -0.5788404596526515
+    }, {
+      "time" : 2.582700817287668,
+      "velocity" : 1.9074773532032985,
+      "acceleration" : 0.5894907207875506,
+      "pose" : {
+        "translation" : {
+          "x" : 2.4995350600901283,
+          "y" : 2.336235732335009
+        },
+        "rotation" : {
+          "radians" : 0.5275249535906203
+        }
+      },
+      "curvature" : -0.5584765250518172
+    }, {
+      "time" : 2.629678599279987,
+      "velocity" : 1.935170319770951,
+      "acceleration" : 0.49960482001977663,
+      "pose" : {
+        "translation" : {
+          "x" : 2.578633685796376,
+          "y" : 2.3797120544804784
+        },
+        "rotation" : {
+          "radians" : 0.47785770284077494
+        }
+      },
+      "curvature" : -0.5426069030826571
+    }, {
+      "time" : 2.6764059539315443,
+      "velocity" : 1.9585155313816427,
+      "acceleration" : 0.46356554662244476,
+      "pose" : {
+        "translation" : {
+          "x" : 2.6604139441662618,
+          "y" : 2.4195577493831344
+        },
+        "rotation" : {
+          "radians" : 0.42909391062732893
+        }
+      },
+      "curvature" : -0.5297484122773238
+    }, {
+      "time" : 2.7228220905005966,
+      "velocity" : 1.9800324531023774,
+      "acceleration" : 0.4993320852380286,
+      "pose" : {
+        "translation" : {
+          "x" : 2.7444234324149193,
+          "y" : 2.455578289033724
+        },
+        "rotation" : {
+          "radians" : 0.3811949720767674
+        }
+      },
+      "curvature" : -0.5182974673057555
+    }, {
+      "time" : 2.7688136027162047,
+      "velocity" : 2.0029974908002472,
+      "acceleration" : 0.6304938957131165,
+      "pose" : {
+        "translation" : {
+          "x" : 2.8302209842882924,
+          "y" : 2.487641014836651
+        },
+        "rotation" : {
+          "radians" : 0.33424953905468247
+        }
+      },
+      "curvature" : -0.5064806911118291
+    }, {
+      "time" : 2.8142032161774035,
+      "velocity" : 2.031615365016311,
+      "acceleration" : 0.8939968521835171,
+      "pose" : {
+        "translation" : {
+          "x" : 2.9173858287977055,
+          "y" : 2.5156837495742366
+        },
+        "rotation" : {
+          "radians" : 0.28849219299419626
+        }
+      },
+      "curvature" : -0.4923123451071928
+    }, {
+      "time" : 2.85873622335247,
+      "velocity" : 2.0714277332490862,
+      "acceleration" : 1.3565333753864743,
+      "pose" : {
+        "translation" : {
+          "x" : 3.005526748954434,
+          "y" : 2.539723409370982
+        },
+        "rotation" : {
+          "radians" : 0.24432259326770525
+        }
+      },
+      "curvature" : -0.4735699432966268
+    }, {
+      "time" : 2.902062698889524,
+      "velocity" : 2.1302015433529657,
+      "acceleration" : 2.153155459162207,
+      "pose" : {
+        "translation" : {
+          "x" : 3.094291240504276,
+          "y" : 2.5598646156578297
+        },
+        "rotation" : {
+          "radians" : 0.20232364570316902
+        }
+      },
+      "curvature" : -0.4477981691549623
+    }, {
+      "time" : 2.9437117576976832,
+      "velocity" : 2.2198784416947213,
+      "acceleration" : 3.592461951491249,
+      "pose" : {
+        "translation" : {
+          "x" : 3.183374670662122,
+          "y" : 2.5763083071364257
+        },
+        "rotation" : {
+          "radians" : 0.1632767221779342
+        }
+      },
+      "curvature" : -0.4123493520484391
+    }, {
+      "time" : 2.9830497106114113,
+      "velocity" : 2.3611985407868445,
+      "acceleration" : 4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 3.2725294368465256,
+          "y" : 2.5893603517433803
+        },
+        "rotation" : {
+          "radians" : 0.12817160720952014
+        }
+      },
+      "curvature" : -0.36446740220852936
+    }, {
+      "time" : 3.0198548135911913,
+      "velocity" : 2.508418952705964,
+      "acceleration" : 4.000000000000005,
+      "pose" : {
+        "translation" : {
+          "x" : 3.3615741254142755,
+          "y" : 2.599440158614531
+        },
+        "rotation" : {
+          "radians" : 0.0982084966435118
+        }
+      },
+      "curvature" : -0.30142499142103213
+    }, {
+      "time" : 3.0544441058745386,
+      "velocity" : 2.646776121839353,
+      "acceleration" : 3.99999999999999,
+      "pose" : {
+        "translation" : {
+          "x" : 3.450402670394965,
+          "y" : 2.6070892900492026
+        },
+        "rotation" : {
+          "radians" : 0.07478856387691135
+        }
+      },
+      "curvature" : -0.22073732623662828
+    }, {
+      "time" : 3.087179502133278,
+      "velocity" : 2.7777177068743097,
+      "acceleration" : 4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.538993512225563,
+          "y" : 2.6129800734744713
+        },
+        "rotation" : {
+          "radians" : 0.05948748242671234
+        }
+      },
+      "curvature" : -0.1205067561761745
+    }, {
+      "time" : 3.118362856162708,
+      "velocity" : 2.9024511229920305,
+      "acceleration" : 3.2578770345335637,
+      "pose" : {
+        "translation" : {
+          "x" : 3.6274187564849854,
+          "y" : 2.617924213409424
+        },
+        "rotation" : {
+          "radians" : 0.0540017456962338
+        }
+      },
+      "curvature" : 0.0
+    }, {
+      "time" : 3.148305323595932,
+      "velocity" : 3.0,
+      "acceleration" : -1.0150458753684738E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 3.715657867815885,
+          "y" : 2.6226762181125665
+        },
+        "rotation" : {
+          "radians" : 0.05341425500982433
+        }
+      },
+      "curvature" : -0.012753175869738934
+    }, {
+      "time" : 3.1774724263267227,
+      "velocity" : 2.9999999999999996,
+      "acceleration" : 1.0337019039886706E-14,
+      "pose" : {
+        "translation" : {
+          "x" : 3.803037680920397,
+          "y" : 2.6272856863245124
+        },
+        "rotation" : {
+          "radians" : 0.05184775131197311
+        }
+      },
+      "curvature" : -0.022669788781850372
+    }, {
+      "time" : 3.2061131274893104,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.888849129236597,
+          "y" : 2.631644952964443
+        },
+        "rotation" : {
+          "radians" : 0.04955597015846323
+        }
+      },
+      "curvature" : -0.03035674043346665
+    }, {
+      "time" : 3.2340415748283977,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 3.9725371670283494,
+          "y" : 2.6356817669948214
+        },
+        "rotation" : {
+          "radians" : 0.04676376501044413
+        }
+      },
+      "curvature" : -0.03596921557348524
+    }, {
+      "time" : 3.261120207908762,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.053689914296321,
+          "y" : 2.639356401176549
+        },
+        "rotation" : {
+          "radians" : 0.043689807233283716
+        }
+      },
+      "curvature" : -0.03932125691085411
+    }, {
+      "time" : 3.2872560287661248,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.1320278016889915,
+          "y" : 2.642658761824123
+        },
+        "rotation" : {
+          "radians" : 0.040562671415580655
+        }
+      },
+      "curvature" : -0.039940377095716434
+    }, {
+      "time" : 3.3123968620373043,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.207392715413668,
+          "y" : 2.6456054985607906
+        },
+        "rotation" : {
+          "radians" : 0.03763198896608686
+        }
+      },
+      "curvature" : -0.03710839155892508
+    }, {
+      "time" : 3.336527620370588,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.279737142147496,
+          "y" : 2.6482371140737087
+        },
+        "rotation" : {
+          "radians" : 0.035174735467647336
+        }
+      },
+      "curvature" : -0.02992149474512993
+    }, {
+      "time" : 3.359666591601313,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.349113313948472,
+          "y" : 2.6506150738690977
+        },
+        "rotation" : {
+          "radians" : 0.033495618460887784
+        }
+      },
+      "curvature" : -0.017400083112939065
+    }, {
+      "time" : 3.3818617651034137,
+      "velocity" : 3.0,
+      "acceleration" : 0.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.415662353166454,
+          "y" : 2.6528189160273996
+        },
+        "rotation" : {
+          "radians" : 0.03291987664459664
+        }
+      },
+      "curvature" : 0.0013263625292214375
+    }, {
+      "time" : 3.423739530881724,
+      "velocity" : 3.0,
+      "acceleration" : -0.8175000091130344,
+      "pose" : {
+        "translation" : {
+          "x" : 4.5412228441782645,
+          "y" : 2.657095421156555
+        },
+        "rotation" : {
+          "radians" : 0.0363729513028002
+        }
+      },
+      "curvature" : 0.05866331262890874
+    }, {
+      "time" : 3.4632152976715145,
+      "velocity" : 2.967728560289602,
+      "acceleration" : -1.882760480943012,
+      "pose" : {
+        "translation" : {
+          "x" : 4.658912906437536,
+          "y" : 2.6619545562850817
+        },
+        "rotation" : {
+          "radians" : 0.047679751894998845
+        }
+      },
+      "curvature" : 0.13592977249614052
+    }, {
+      "time" : 3.5018404054419703,
+      "velocity" : 2.8950067338072225,
+      "acceleration" : -1.9238632002001914,
+      "pose" : {
+        "translation" : {
+          "x" : 4.771954350173473,
+          "y" : 2.668388433754444
+        },
+        "rotation" : {
+          "radians" : 0.0674512871959646
+        }
+      },
+      "curvature" : 0.2106703311228447
+    }, {
+      "time" : 3.541164793474475,
+      "velocity" : 2.8193519908010933,
+      "acceleration" : -3.999999999999996,
+      "pose" : {
+        "translation" : {
+          "x" : 4.883949141742278,
+          "y" : 2.677400863588673
+        },
+        "rotation" : {
+          "radians" : 0.09396342350209999
+        }
+      },
+      "curvature" : 0.2540441775959243
+    }, {
+      "time" : 3.583307920417398,
+      "velocity" : 2.6507794830294023,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 4.998532040779537,
+          "y" : 2.689914865659375
+        },
+        "rotation" : {
+          "radians" : 0.12355864176280444
+        }
+      },
+      "curvature" : 0.25210444079517663
+    }, {
+      "time" : 3.630910520336883,
+      "velocity" : 2.460369083351462,
+      "acceleration" : -3.999999999999993,
+      "pose" : {
+        "translation" : {
+          "x" : 5.119023237352621,
+          "y" : 2.706680181850743
+        },
+        "rotation" : {
+          "radians" : 0.1521781406035511
+        }
+      },
+      "curvature" : 0.21456009552095912
+    }, {
+      "time" : 3.6575544532910382,
+      "velocity" : 2.3537933515348413,
+      "acceleration" : -3.9999999999999933,
+      "pose" : {
+        "translation" : {
+          "x" : 5.182350507669064,
+          "y" : 2.7168212357026604
+        },
+        "rotation" : {
+          "radians" : 0.16512628632297052
+        }
+      },
+      "curvature" : 0.18899817754880754
+    }, {
+      "time" : 3.6866111335997718,
+      "velocity" : 2.237566630299908,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.2480809891130775,
+          "y" : 2.728180788224563
+        },
+        "rotation" : {
+          "radians" : 0.1768351792035555
+        }
+      },
+      "curvature" : 0.16220610323939916
+    }, {
+      "time" : 3.718563991190063,
+      "velocity" : 2.109755199938744,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.3163865477339485,
+          "y" : 2.740762636728995
+        },
+        "rotation" : {
+          "radians" : 0.1871757577997712
+        }
+      },
+      "curvature" : 0.13593088502288128
+    }, {
+      "time" : 3.754021980661885,
+      "velocity" : 1.967923242051456,
+      "acceleration" : -4.000000000000003,
+      "pose" : {
+        "translation" : {
+          "x" : 5.387354258449022,
+          "y" : 2.7545424071852267
+        },
+        "rotation" : {
+          "radians" : 0.19609326514864456
+        }
+      },
+      "curvature" : 0.11126066622925063
+    }, {
+      "time" : 3.7938015783178165,
+      "velocity" : 1.8088048514277302,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.460975549954718,
+          "y" : 2.7694646639744107
+        },
+        "rotation" : {
+          "radians" : 0.20358486920868646
+        }
+      },
+      "curvature" : 0.08871019500715766
+    }, {
+      "time" : 3.8390908801882153,
+      "velocity" : 1.627647643946134,
+      "acceleration" : -3.9999999999999973,
+      "pose" : {
+        "translation" : {
+          "x" : 5.537135349637538,
+          "y" : 2.7854400196447386
+        },
+        "rotation" : {
+          "radians" : 0.20967774359389213
+        }
+      },
+      "curvature" : 0.06834921616165193
+    }, {
+      "time" : 3.8918213921686315,
+      "velocity" : 1.4167255960244693,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.615601228485083,
+          "y" : 2.8023422446665975
+        },
+        "rotation" : {
+          "radians" : 0.21440925415138234
+        }
+      },
+      "curvature" : 0.049921154171936793
+    }, {
+      "time" : 3.955692162693215,
+      "velocity" : 1.1612425139261353,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.696012545997064,
+          "y" : 2.8200053771877265
+        },
+        "rotation" : {
+          "radians" : 0.21780924678420455
+        }
+      },
+      "curvature" : 0.03292187698251918
+    }, {
+      "time" : 4.040210129462593,
+      "velocity" : 0.8231706468486228,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.777869595096313,
+          "y" : 2.838220832788373
+        },
+        "rotation" : {
+          "radians" : 0.21988311898862645
+        }
+      },
+      "curvature" : 0.016620346183280694
+    }, {
+      "time" : 4.246002791174749,
+      "velocity" : 0.0,
+      "acceleration" : -4.0,
+      "pose" : {
+        "translation" : {
+          "x" : 5.860522747039795,
+          "y" : 2.85673451423645
+        },
+        "rotation" : {
+          "radians" : 0.22059308726779464
+        }
+      },
+      "curvature" : 0.0
+    } ],
+    "pointList" : [ {
+      "x" : [ 4.100522994995117, -3.486077070236206, 0.0 ],
+      "y" : [ -0.7109201550483704, 2.165250778198242, 0.0 ]
+    }, {
+      "x" : [ 1.8818800449371338, -0.19111382961273193, 0.0 ],
+      "y" : [ 1.2418794631958008, 1.7965017557144165, 0.0 ]
+    }, {
+      "x" : [ 3.6274187564849854, 2.828535318374634, 0.0 ],
+      "y" : [ 2.617924213409424, 0.1528944969177246, 0.0 ]
+    }, {
+      "x" : [ 5.860522747039795, 2.649440288543701, 0.0 ],
+      "y" : [ 2.85673451423645, 0.5941164493560791, 0.0 ]
+    } ],
+    "rotations" : [ {
+      "time" : 0.0,
+      "rotation" : {
+        "radians" : 2.584307420515759
+      }
+    }, {
+      "time" : 1.699999962002039,
+      "rotation" : {
+        "radians" : 2.7676508674151563
+      }
+    }, {
+      "time" : 3.1199999302625656,
+      "rotation" : {
+        "radians" : 0.4494112193892231
+      }
+    }, {
+      "time" : 4.159999907016754,
+      "rotation" : {
+        "radians" : 0.46364803834258117
+      }
+    } ],
+    "reversed" : false,
+    "color" : 286.3529,
+    "closed" : false,
+    "velocityStart" : 0.0,
+    "velocityEnd" : 0.0,
+    "constraints" : [ ]
+  } ]
+}
\ No newline at end of file
diff --git a/src/main/deploy/shooter/shooterconfig.json b/src/main/deploy/shooter/shooterconfig.json
index c1d58560..69ec53f0 100644
--- a/src/main/deploy/shooter/shooterconfig.json
+++ b/src/main/deploy/shooter/shooterconfig.json
@@ -1,28 +1,32 @@
 {
   "shooterConfigs" : [ {
-    "hoodEjectAngle" : 68.0,
-    "flywheelSpeed" : 1700.0,
-    "distance" : 40.0
+    "hoodEjectAngle" : 70.0,
+    "flywheelSpeed" : 1800.0,
+    "distance" : 45.0
   }, {
     "hoodEjectAngle" : 62.0,
     "flywheelSpeed" : 1700.0,
-    "distance" : 60.0
+    "distance" : 62.0
   }, {
     "hoodEjectAngle" : 56.0,
     "flywheelSpeed" : 1700.0,
-    "distance" : 82.0
+    "distance" : 81.0
   }, {
     "hoodEjectAngle" : 56.0,
-    "flywheelSpeed" : 1800.0,
-    "distance" : 106.0
+    "flywheelSpeed" : 1900.0,
+    "distance" : 101.0
   }, {
     "hoodEjectAngle" : 53.0,
-    "flywheelSpeed" : 1800.0,
-    "distance" : 115.0
+    "flywheelSpeed" : 1900.0,
+    "distance" : 109.0
   }, {
     "hoodEjectAngle" : 51.0,
     "flywheelSpeed" : 1900.0,
-    "distance" : 120.0
+    "distance" : 113.0
+  }, {
+    "hoodEjectAngle" : 51.0,
+    "flywheelSpeed" : 1900.0,
+    "distance" : 124.0
   }, {
     "hoodEjectAngle" : 51.0,
     "flywheelSpeed" : 2000.0,
@@ -34,38 +38,38 @@
   }, {
     "hoodEjectAngle" : 51.0,
     "flywheelSpeed" : 2200.0,
-    "distance" : 157.0
+    "distance" : 160.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2200.0,
-    "distance" : 175.0
+    "flywheelSpeed" : 2300.0,
+    "distance" : 173.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2300.0,
-    "distance" : 190.0
+    "flywheelSpeed" : 2400.0,
+    "distance" : 181.0
   }, {
     "hoodEjectAngle" : 51.0,
     "flywheelSpeed" : 2400.0,
-    "distance" : 200.0
+    "distance" : 190.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2500.0,
-    "distance" : 210.0
+    "flywheelSpeed" : 2450.0,
+    "distance" : 202.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2600.0,
-    "distance" : 225.0
+    "flywheelSpeed" : 2550.0,
+    "distance" : 224.0
   }, {
     "hoodEjectAngle" : 51.0,
     "flywheelSpeed" : 2700.0,
-    "distance" : 250.0
+    "distance" : 241.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2800.0,
-    "distance" : 270.0
+    "flywheelSpeed" : 3000.0,
+    "distance" : 265.0
   }, {
     "hoodEjectAngle" : 51.0,
-    "flywheelSpeed" : 2900.0,
-    "distance" : 299.0
+    "flywheelSpeed" : 3200.0,
+    "distance" : 300.0
   } ]
 }
\ No newline at end of file
diff --git a/src/main/java/frc/auton/BuddyAutoLeftHideBlue.java b/src/main/java/frc/auton/BuddyAutoLeftHideBlue.java
new file mode 100644
index 00000000..ef29bba4
--- /dev/null
+++ b/src/main/java/frc/auton/BuddyAutoLeftHideBlue.java
@@ -0,0 +1,12 @@
+package frc.auton;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import frc.auton.guiauto.AbstractGuiAuto;
+
+import java.io.File;
+
+public class BuddyAutoLeftHideBlue extends AbstractGuiAuto {
+    public BuddyAutoLeftHideBlue() {
+        super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/blue/buddyautoleftsteal.json"));
+    }
+}
diff --git a/src/main/java/frc/auton/BuddyAutoLeftHideRed.java b/src/main/java/frc/auton/BuddyAutoLeftHideRed.java
new file mode 100644
index 00000000..b39f57e0
--- /dev/null
+++ b/src/main/java/frc/auton/BuddyAutoLeftHideRed.java
@@ -0,0 +1,12 @@
+package frc.auton;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import frc.auton.guiauto.AbstractGuiAuto;
+
+import java.io.File;
+
+public class BuddyAutoLeftHideRed extends AbstractGuiAuto {
+    public BuddyAutoLeftHideRed() {
+        super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/red/buddyautoleftsteal.json"));
+    }
+}
diff --git a/src/main/java/frc/auton/HangerCargoBlue.java b/src/main/java/frc/auton/HangerCargoBlue.java
new file mode 100644
index 00000000..45c62df2
--- /dev/null
+++ b/src/main/java/frc/auton/HangerCargoBlue.java
@@ -0,0 +1,12 @@
+package frc.auton;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import frc.auton.guiauto.AbstractGuiAuto;
+
+import java.io.File;
+
+public class HangerCargoBlue extends AbstractGuiAuto {
+    public HangerCargoBlue() {
+        super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/blue/movecargotohanger.json"));
+    }
+}
diff --git a/src/main/java/frc/auton/HangerCargoRed.java b/src/main/java/frc/auton/HangerCargoRed.java
new file mode 100644
index 00000000..fee9b47f
--- /dev/null
+++ b/src/main/java/frc/auton/HangerCargoRed.java
@@ -0,0 +1,12 @@
+package frc.auton;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import frc.auton.guiauto.AbstractGuiAuto;
+
+import java.io.File;
+
+public class HangerCargoRed extends AbstractGuiAuto {
+    public HangerCargoRed() {
+        super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/red/movecargotohanger.json"));
+    }
+}
diff --git a/src/main/java/frc/auton/HideCargoBlue.java b/src/main/java/frc/auton/HideCargoBlue.java
new file mode 100644
index 00000000..56f9a93b
--- /dev/null
+++ b/src/main/java/frc/auton/HideCargoBlue.java
@@ -0,0 +1,12 @@
+package frc.auton;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import frc.auton.guiauto.AbstractGuiAuto;
+
+import java.io.File;
+
+public class HideCargoBlue extends AbstractGuiAuto {
+    public HideCargoBlue() {
+        super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/blue/hidecargo.json"));
+    }
+}
diff --git a/src/main/java/frc/auton/HideCargoRed.java b/src/main/java/frc/auton/HideCargoRed.java
new file mode 100644
index 00000000..07a69620
--- /dev/null
+++ b/src/main/java/frc/auton/HideCargoRed.java
@@ -0,0 +1,12 @@
+package frc.auton;
+
+import edu.wpi.first.wpilibj.Filesystem;
+import frc.auton.guiauto.AbstractGuiAuto;
+
+import java.io.File;
+
+public class HideCargoRed extends AbstractGuiAuto {
+    public HideCargoRed() {
+        super(new File(Filesystem.getDeployDirectory().getPath() + "/autos/red/hidecargo.json"));
+    }
+}
diff --git a/src/main/java/frc/auton/guiauto/serialization/reflection/ClassInformationSender.java b/src/main/java/frc/auton/guiauto/serialization/reflection/ClassInformationSender.java
index 30419729..8984d0ef 100644
--- a/src/main/java/frc/auton/guiauto/serialization/reflection/ClassInformationSender.java
+++ b/src/main/java/frc/auton/guiauto/serialization/reflection/ClassInformationSender.java
@@ -2,14 +2,13 @@
 
 import edu.wpi.first.wpilibj.Filesystem;
 import frc.utility.Serializer;
+import org.jetbrains.annotations.Nullable;
 
 import java.io.File;
 import java.io.IOException;
 import java.util.ArrayList;
 import java.util.List;
 
-import org.jetbrains.annotations.Nullable;
-
 import static frc.auton.guiauto.serialization.reflection.ReflectionUtils.findClasses;
 public final class ClassInformationSender {
     public ClassInformationSender() {
@@ -23,10 +22,10 @@ public static void updateReflectionInformation(@Nullable File file) {
             for (Class<?> aClass : classes) {
                 reflectionClassData.add(new ReflectionClassData(aClass));
             }
-            System.out.println(Serializer.serializeToString(reflectionClassData));
-
+            //System.out.println(Serializer.serializeToString(reflectionClassData));
+            System.out.println("Successfully updated reflection information");
 
-            if(file != null) {
+            if (file != null) {
                 file.getParentFile().mkdir();
                 Serializer.serializeToFile(reflectionClassData, file);
             }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 511631b9..c1a3c762 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -42,7 +42,7 @@ public final class Constants {
      * Relative position of the limelight from the center of the robot.
      */
     public static final Translation2d LIMELIGHT_CENTER_OFFSET = new Translation2d(-0.684, 0); //TODO: CHANGE
-    public static final double VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED = Math.pow(1.5, 2); //TODO: CHANGE
+    public static final double VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED = Math.pow(2, 2); //TODO: CHANGE
     // This is in inches
     public static final double VISION_DISTANCE_BEFORE_ERROR_TIGHTENING = 200;
 
@@ -164,9 +164,9 @@ public final class Constants {
 
     // TurnPID
 
-    public static final double DEFAULT_TURN_P = 8.0;
-    public static final double DEFAULT_TURN_I = 0.0;
-    public static final double DEFAULT_TURN_D = 0.0;
+    public static final double DEFAULT_TURN_P = 12.0;
+    public static final double DEFAULT_TURN_I = 15.0;
+    public static final double DEFAULT_TURN_D = 0.4;
     public static final double DEFAULT_TURN_MAX_VELOCITY = 10.0;
     public static final double DEFAULT_TURN_MAX_ACCELERATION = 10.0;
 
@@ -207,7 +207,7 @@ public final class Constants {
      */
     public static final double MAX_SHOOT_SPEED_SQUARED = Math.pow(MAX_SHOOT_SPEED, 2);
 
-   /**
+    /**
      * This GOAL RADIUS is used to calculate the turn error
      */
     public static final double GOAL_RADIUS_TURN_ERROR_M = GOAL_RADIUS * 3;
@@ -228,11 +228,38 @@ public final class Constants {
 
 
     //Hopper Constants
-    public static final int HOPPER_PERIOD = 200;
-    public static final double HOPPER_SPEED = 1;
+    public static final int HOPPER_PERIOD = 10;
+    public static final double HOPPER_SPEED = 1.0;
     public static final int HOPPER_MOTOR_ID = 30;
     public static final int HOPPER_CURRENT_LIMIT = 35;
+    public static final String INTAKE_LIMELIGHT_NAME = "limelight-intake";
+
+    // Outtake Constants
+    public static final int OUTTAKE_CAN_ID = 60;
+    public static final double OUTTAKE_REDUCTION = (3.0 / 1.0);
+    public static final int OUTTAKE_CURRENT_LIMIT = 20;
+    public static final int OUTTAKE_MEASUREMENT_PERIOD_MS = 200;
+    public static final double OUTTAKE_VERTICAL_OFFSET_THRESHOLD = 0;
 
+    /**
+     * Outtake's speed when automatically ejecting balls
+     */
+    public static final double OUTTAKE_AUTO_EJECTION_SPEED = .5;
+
+    /**
+     * Outtake's speed when manually ejecting balls
+     */
+    public static final double OUTTAKE_MANUAL_EJECTION_SPEED = 1;
+
+    /**
+     * Outtake's speed when intaking balls
+     */
+    public static final double OUTTAKE_SPEED = -.5;
+
+    /**
+     * Outtake will run for this time (Sec) after color sensor trigger
+     */
+    public static final double OUTTAKE_RUN_PERIOD = 50 / 1000d;
 
     // Shooter Constants
 
@@ -262,6 +289,9 @@ public final class Constants {
      */
     public static final double ALLOWED_SHOOTER_SPEED_ERROR_RPM = 150;
 
+    public static final int SHOOTER_EJECT_SPEED = 750;
+    public static final double HOOD_EJECT_ANGLE = 53;
+
     /**
      * Conversion from Falcon Sensor Units / 100ms to RPM 2048 is Sensor Units Per Revolution 600 Converts From Time of 100ms to 1
      * minute
@@ -478,7 +508,7 @@ public final class Constants {
     /**
      * Length to grab on high and traversal bars
      */
-    public static final double CLIMBER_GRAB_ON_NEXT_BAR_EXTENSION = (27.2 - 4.5) * CLIMBER_ENCODER_TICKS_PER_INCH;
+    public static final double CLIMBER_GRAB_ON_NEXT_BAR_EXTENSION = (27.2 - 3.5) * CLIMBER_ENCODER_TICKS_PER_INCH;
 
     /**
      * How long only one of the sensor switches can be closed for before the climb will pause
@@ -492,9 +522,19 @@ public final class Constants {
     public static final int INTAKE_PERIOD = 50;
     public static final int INTAKE_SOLENOID_CHANNEL = 3;
     public static final int INTAKE_MOTOR_DEVICE_ID = 40;
-    public static final double INTAKE_MOTOR_SPEED = -1;
     public static final double INTAKE_OPEN_TIME = 0.0;
 
+    /**
+     * Intake Speed when intaking balls
+     */
+    public static final double INTAKE_SPEED = -1;
+
+    /**
+     * Intake speed when ejecting balls
+     */
+    public static final double INTAKE_EJECTION_SPEED = -.5;
+
     public static final int WEB_DASHBOARD_PORT = 5802;
+
     public static final int WEB_DASHBOARD_SEND_PERIOD_MS = 50;
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index a7f0d95d..d47d44a3 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -27,12 +27,14 @@
 import frc.utility.Limelight.StreamingMode;
 import frc.utility.shooter.visionlookup.ShooterConfig;
 import frc.utility.shooter.visionlookup.ShooterPreset;
+import frc.utility.shooter.visionlookup.VisionLookUpTable;
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;
 
 import java.io.File;
 import java.io.IOException;
 import java.lang.Thread.State;
+import java.nio.file.Files;
 import java.util.Collections;
 import java.util.concurrent.CompletableFuture;
 import java.util.concurrent.ExecutorService;
@@ -52,6 +54,8 @@ public class Robot extends TimedRobot {
 
     public boolean useFieldRelative = false;
 
+    public static final boolean IS_PRACTICE = Files.exists(new File("/home/lvuser/practice").toPath());
+
     //GUI
     final @NotNull NetworkTableInstance instance = NetworkTableInstance.getDefault();
     final @NotNull NetworkTable autoDataTable = instance.getTable("autodata");
@@ -122,6 +126,31 @@ public class Robot extends TimedRobot {
     @NotNull
     private BuddyAutoRight buddyAutoRight;
 
+    @SuppressWarnings("NotNullFieldNotInitialized")
+    @NotNull
+    private HideCargoBlue hideCargoBlue;
+
+    @SuppressWarnings("NotNullFieldNotInitialized")
+    @NotNull
+    private HideCargoRed hideCargoRed;
+
+    @SuppressWarnings("NotNullFieldNotInitialized")
+    @NotNull
+    private BuddyAutoLeftHideBlue buddyAutoLeftHideBlue;
+
+    @SuppressWarnings("NotNullFieldNotInitialized")
+    @NotNull
+    private BuddyAutoLeftHideRed buddyAutoLeftHideRed;
+
+    @SuppressWarnings("NotNullFieldNotInitialized")
+    @NotNull
+    private HangerCargoBlue hangerCargoBlue;
+
+    @SuppressWarnings("NotNullFieldNotInitialized")
+    @NotNull
+    private HangerCargoRed hangerCargoRed;
+
+
     @NotNull private static final String SHOOT_AND_MOVE_LEFT = "Shoot and Move Left";
     @NotNull private static final String SHOOT_AND_MOVE_MID = "Shoot and Move Mid";
     @NotNull private static final String SHOOT_AND_MOVE_RIGHT = "Shoot and Move Right";
@@ -133,27 +162,19 @@ public class Robot extends TimedRobot {
     @NotNull private static final String SHOOT_ONLY_RIGHT = "Shoot Only Right";
     @NotNull private static final String SHOOT_ONLY_MID = "Shoot Only Mid";
     @NotNull private static final String SHOOT_ONLY_LEFT = "Shoot Only Left";
+    @NotNull private static final String HIDE_CARGO = "Hide Cargo";
+    @NotNull private static final String BUDDY_AUTO_LEFT_HIDE = "Buddy Auto Left Hide";
+    @NotNull private static final String HANGER_CARGO = "Hanger Cargo";
+
 
     private static final String RESET_POSE = "Reset Pose";
 
     private final SendableChooser<String> autoChooser = new SendableChooser<>();
 
+    @NotNull public static final String RED = "RED";
+    @NotNull public static final String BLUE = "BLUE";
 
-    @NotNull private static final String RED = "RED";
-    @NotNull private static final String BLUE = "BLUE";
-
-    private final SendableChooser<String> sideChooser = new SendableChooser<>();
-
-    //Subsystems
-    private final RobotTracker robotTracker = RobotTracker.getInstance();
-    private final Drive drive = Drive.getInstance();
-    private final BlinkinLED blinkinLED = BlinkinLED.getInstance();
-    private final Limelight limelight = Limelight.getInstance();
-    private final Hopper hopper = Hopper.getInstance();
-    private final Intake intake = Intake.getInstance();
-    private final Shooter shooter = Shooter.getInstance();
-    private final Climber climber = Climber.getInstance();
-    private final VisionManager visionManager = VisionManager.getInstance();
+    public static final SendableChooser<String> sideChooser = new SendableChooser<>();
 
     //Inputs
     private final static Controller xbox = new Controller(0);
@@ -205,7 +226,7 @@ public static void setRumble(RumbleType type, double value) {
                             ShooterConfig shooterConfig = (ShooterConfig) Serializer.deserialize(shooterConfigEntry.getString(null),
                                     ShooterConfig.class);
                             Collections.sort(shooterConfig.getShooterConfigs());
-                            visionManager.setShooterConfig(shooterConfig);
+                            VisionManager.getInstance().setShooterConfig(shooterConfig);
                             System.out.println(shooterConfig.getShooterConfigs());
                         } catch (IOException e) {
                             //Should never happen. The gui should never upload invalid data.
@@ -224,6 +245,10 @@ public static void setRumble(RumbleType type, double value) {
      */
     @Override
     public void robotInit() {
+        final RobotTracker robotTracker = RobotTracker.getInstance();
+        final Limelight limelight = Limelight.getInstance();
+        final Limelight intakeLimelight = Limelight.getInstance(Constants.INTAKE_LIMELIGHT_NAME);
+
 
         if (autoPath.getString(null) != null) {
             autoPathListener.accept(new EntryNotification(NetworkTableInstance.getDefault(), 1, 1, "", null, 12));
@@ -251,6 +276,13 @@ public void robotInit() {
         CompletableFuture.runAsync(() -> fiveBallRed = new FiveBallRed()).thenRun(this::incrementLoadedAutos);
         CompletableFuture.runAsync(() -> sixBallRed = new SixBallRed()).thenRun(this::incrementLoadedAutos);
         CompletableFuture.runAsync(() -> buddyAutoLeft = new BuddyAutoLeft()).thenRun(this::incrementLoadedAutos);
+        CompletableFuture.runAsync(() -> hideCargoBlue = new HideCargoBlue()).thenRun(this::incrementLoadedAutos);
+        CompletableFuture.runAsync(() -> hideCargoRed = new HideCargoRed()).thenRun(this::incrementLoadedAutos);
+        CompletableFuture.runAsync(() -> buddyAutoLeftHideBlue = new BuddyAutoLeftHideBlue()).thenRun(this::incrementLoadedAutos);
+        CompletableFuture.runAsync(() -> buddyAutoLeftHideRed = new BuddyAutoLeftHideRed()).thenRun(this::incrementLoadedAutos);
+        CompletableFuture.runAsync(() -> hangerCargoBlue = new HangerCargoBlue()).thenRun(this::incrementLoadedAutos);
+        CompletableFuture.runAsync(() -> hangerCargoRed = new HangerCargoRed()).thenRun(this::incrementLoadedAutos);
+
 
         SmartDashboard.putBoolean("Field Relative Enabled", useFieldRelative);
         autoChooser.setDefaultOption(SHOOT_AND_MOVE_LEFT, SHOOT_AND_MOVE_LEFT);
@@ -263,6 +295,9 @@ public void robotInit() {
         autoChooser.addOption(SHOOT_ONLY_LEFT, SHOOT_ONLY_LEFT);
         autoChooser.addOption(SHOOT_ONLY_MID, SHOOT_ONLY_MID);
         autoChooser.addOption(SHOOT_ONLY_RIGHT, SHOOT_ONLY_RIGHT);
+        autoChooser.addOption(HIDE_CARGO, HIDE_CARGO);
+        autoChooser.addOption(HANGER_CARGO, HANGER_CARGO);
+        autoChooser.addOption(BUDDY_AUTO_LEFT_HIDE, BUDDY_AUTO_LEFT_HIDE);
 
         sideChooser.setDefaultOption(BLUE, BLUE);
         sideChooser.addOption(RED, RED);
@@ -270,7 +305,6 @@ public void robotInit() {
         SmartDashboard.putData("Auto choices", autoChooser);
         SmartDashboard.putData("Red or Blue", sideChooser);
 
-        startSubsystems();
         robotTracker.resetGyro();
         OrangeUtility.sleep(50);
         robotTracker.resetPosition(new Pose2d());
@@ -284,7 +318,11 @@ public void robotInit() {
 
         NetworkTableInstance.getDefault().setUpdateRate(0.05);
         Limelight.getInstance().setStreamingMode(StreamingMode.PIP_SECONDARY);
+        startSubsystems();
         limelight.setLedMode(LedMode.OFF);
+        intakeLimelight.setLedMode(LedMode.OFF);
+
+        limelight.setStreamingMode(StreamingMode.STANDARD);
 //        shooter.homeHood();
 //        shooter.setHoodPositionMode(HoodPositionMode.RELATIVE_TO_HOME);
     }
@@ -294,7 +332,7 @@ public void robotInit() {
     volatile boolean loadingAutos = true;
 
     public void incrementLoadedAutos() {
-        if (loadedAutos.incrementAndGet() == 10) {
+        if (loadedAutos.incrementAndGet() == 16) {
             loadingAutos = false;
         }
     }
@@ -308,17 +346,32 @@ public void incrementLoadedAutos() {
      */
     @Override
     public void robotPeriodic() {
+        VisionManager visionManager = VisionManager.getInstance();
         SmartDashboard.putNumber("Match Timestamp", DriverStation.getMatchTime());
+        if (!DriverStation.isEnabled()) {
+            xbox.update();
+            stick.update();
+            buttonPanel.update();
+        }
+
+        if (stick.getRawButton(4)) {
+            visionManager.forceVisionOn(resettingPoseVisionOn);
+            visionManager.forceUpdatePose();
+        } else {
+            visionManager.unForceVisionOn(resettingPoseVisionOn);
+        }
     }
 
 
     @Override
     public void autonomousInit() {
+        final Drive drive = Drive.getInstance();
+        final Climber climber = Climber.getInstance();
+
         startSubsystems();
         climber.configBrake();
         enabled.setBoolean(true);
         drive.configBrake();
-        drive.resetAuto();
 
         networkAutoLock.lock();
         try {
@@ -357,6 +410,15 @@ public void autonomousInit() {
                         case BUDDY_AUTO_LEFT:
                             selectedAuto = buddyAutoLeft;
                             break;
+                        case HIDE_CARGO:
+                            selectedAuto = hideCargoBlue;
+                            break;
+                        case BUDDY_AUTO_LEFT_HIDE:
+                            selectedAuto = buddyAutoLeftHideBlue;
+                            break;
+                        case HANGER_CARGO:
+                            selectedAuto = hangerCargoBlue;
+                            break;
                         default:
                             selectedAuto = shootAndMoveRight;
                             break;
@@ -393,6 +455,15 @@ public void autonomousInit() {
                         case BUDDY_AUTO_LEFT:
                             selectedAuto = buddyAutoLeft;
                             break;
+                        case HIDE_CARGO:
+                            selectedAuto = hideCargoRed;
+                            break;
+                        case BUDDY_AUTO_LEFT_HIDE:
+                            selectedAuto = buddyAutoLeftHideRed;
+                            break;
+                        case HANGER_CARGO:
+                            selectedAuto = hangerCargoRed;
+                            break;
                         default:
                             selectedAuto = shootAndMoveRight;
                             break;
@@ -427,6 +498,10 @@ public void autonomousPeriodic() {
      */
     @Override
     public void teleopInit() {
+        final Drive drive = Drive.getInstance();
+        final Hopper hopper = Hopper.getInstance();
+        final Climber climber = Climber.getInstance();
+
         startSubsystems();
         climber.configBrake();
         enabled.setBoolean(true);
@@ -434,6 +509,7 @@ public void teleopInit() {
         SmartDashboard.putBoolean("Field Relative Enabled", true);
         drive.useFieldRelative = true;
         SmartDashboard.putBoolean("Drive Field Relative Allowed", true);
+        hopper.setEjectOverride(false);
         drive.configBrake();
     }
 
@@ -445,11 +521,26 @@ public void teleopInit() {
      */
     @Override
     public void teleopPeriodic() {
+        final RobotTracker robotTracker = RobotTracker.getInstance();
+        final Drive drive = Drive.getInstance();
+        final Hopper hopper = Hopper.getInstance();
+        final Intake intake = Intake.getInstance();
+        final Shooter shooter = Shooter.getInstance();
+        final Climber climber = Climber.getInstance();
+
+        VisionManager visionManager = VisionManager.getInstance();
         xbox.update();
         stick.update();
         buttonPanel.update();
 
-        if (xbox.getRawAxis(2) > 0.1) {
+        // Hood Eject
+        if (buttonPanel.getRawButton(6)) {
+            shooter.setFeederChecksDisabled(true);
+            shooter.setSpeed(Constants.SHOOTER_EJECT_SPEED);
+            shooter.setHoodPosition(Constants.HOOD_EJECT_ANGLE);
+            shooter.setFiring(true);
+        } else if (xbox.getRawAxis(2) > 0.1) {
+            shooter.setFeederChecksDisabled(false);
             if (isTryingToRunShooterFromButtonPanel()) { //If vision is off
                 shooter.setHoodPosition(shooterPreset.getHoodEjectAngle());
                 shooter.setSpeed(shooterPreset.getFlywheelSpeed());
@@ -462,6 +553,7 @@ public void teleopPeriodic() {
                 visionManager.shootAndMove(getControllerDriveInputs(), useFieldRelative);
             }
         } else {
+            shooter.setFeederChecksDisabled(false);
             shooter.setFiring(false);
             shooter.setSpeed(0);
             visionManager.unForceVisionOn(driverForcingVisionOn);
@@ -508,19 +600,17 @@ public void teleopPeriodic() {
             robotTracker.resetPosition(new Pose2d(robotTracker.getLastEstimatedPoseMeters().getTranslation(), new Rotation2d(0)));
         }
 
-        if (xbox.getRisingEdge(Controller.XboxButtons.START)) {
+        // Override outtake to always intake
+        if (buttonPanel.getRisingEdge(5)) {
+            hopper.toggleEjectOverride();
+        }
+
+        if (xbox.getRisingEdge(XboxButtons.LEFT_CLICK) || xbox.getRisingEdge(Controller.XboxButtons.START)) {
             useFieldRelative = !useFieldRelative;
             System.out.println("Field relative: " + useFieldRelative);
             SmartDashboard.putBoolean("Field Relative Enabled", useFieldRelative);
         }
 
-        if (stick.getRawButton(4)) {
-            visionManager.forceVisionOn(resettingPoseVisionOn);
-            visionManager.forceUpdatePose();
-        } else {
-            visionManager.unForceVisionOn(resettingPoseVisionOn);
-        }
-
 //        if (xbox.getRisingEdge(XboxButtons.LEFT_BUMPER)) {
 //            visionManager.adjustShooterHoodBias(-0.5);
 //        } else if (xbox.getRisingEdge(XboxButtons.RIGHT_BUMPER)) {
@@ -545,7 +635,7 @@ public void teleopPeriodic() {
             climber.setClimberMotor(0);
         }
 
-        if (buttonPanel.getRisingEdge(12)) {
+        if (buttonPanel.getRisingEdge(12) && buttonPanel.getRawButton(9)) {
             climber.forceAdvanceStep();
         }
 
@@ -590,6 +680,7 @@ public void teleopPeriodic() {
     }
 
     private void runShooter() {
+        VisionManager visionManager = VisionManager.getInstance();
         if (buttonPanel.getRisingEdge(1)) {
             shooterPreset = visionManager.visionLookUpTable.getShooterPreset(299);
         } else if (buttonPanel.getRisingEdge(2)) {
@@ -600,15 +691,16 @@ private void runShooter() {
     }
 
     private boolean isTryingToRunShooterFromButtonPanel() {
-        return buttonPanel.getRawButton(1) || buttonPanel.getRawButton(2) || buttonPanel.getRawButton(3);
+        return buttonPanel.getRawButton(1) || buttonPanel.getRawButton(2) || buttonPanel.getRawButton(
+                3) || buttonPanel.getRawButton(6);
     }
 
     private static final ControllerDriveInputs NO_MOTION_CONTROLLER_INPUTS = new ControllerDriveInputs(0, 0, 0);
 
     private void doNormalDriving() {
-        ControllerDriveInputs controllerDriveInputs = getControllerDriveInputs();
-
+        final Drive drive = Drive.getInstance();
 
+        ControllerDriveInputs controllerDriveInputs = getControllerDriveInputs();
         if (controllerDriveInputs.getX() == 0 && controllerDriveInputs.getY() == 0 && controllerDriveInputs.getRotation() == 0
                 && drive.getSpeedSquared() < 0.1) {
             if (xbox.getRawButton(XboxButtons.Y)) {
@@ -626,12 +718,22 @@ private void doNormalDriving() {
     }
 
     private ControllerDriveInputs getControllerDriveInputs() {
-        if (xbox.getRawButton(Controller.XboxButtons.X)) {
-            return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0),
-                    -xbox.getRawAxis(4)).applyDeadZone(0.2, 0.2, 0.2, 0.2).squareInputs();
+        if (useFieldRelative) {
+            if (xbox.getRawButton(Controller.XboxButtons.X)) {
+                return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0),
+                        -xbox.getRawAxis(4)).applyDeadZone(0.2, 0.2, 0.2, 0.2).squareInputs();
+            } else {
+                return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0),
+                        -xbox.getRawAxis(4)).applyDeadZone(0.05, 0.05, 0.2, 0.2).squareInputs();
+            }
         } else {
-            return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0),
-                    -xbox.getRawAxis(4)).applyDeadZone(0.05, 0.05, 0.2, 0.2).squareInputs();
+            if (xbox.getRawButton(Controller.XboxButtons.X)) {
+                return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0),
+                        -xbox.getRawAxis(4)).applyDeadZone(0.2, 0.2, 0.2, 0.2).squareInputs();
+            } else {
+                return new ControllerDriveInputs(-xbox.getRawAxis(1), -xbox.getRawAxis(0),
+                        -xbox.getRawAxis(4)).applyDeadZone(0.05, 0.05, 0.2, 0.2).squareInputs();
+            }
         }
     }
 
@@ -641,7 +743,7 @@ private ControllerDriveInputs getControllerDriveInputs() {
     @Override
     public void disabledInit() {
         killAuto();
-        drive.configCoast();
+        Drive.getInstance().configCoast();
         enabled.setBoolean(false);
     }
 
@@ -657,7 +759,7 @@ public void disabledPeriodic() {
      */
     @Override
     public void testInit() {
-        drive.configCoast();
+        Drive.getInstance().configCoast();
     }
 
     /**
@@ -669,6 +771,12 @@ public void testPeriodic() {
         stick.update();
         buttonPanel.update();
 
+        final Drive drive = Drive.getInstance();
+        final Hopper hopper = Hopper.getInstance();
+        final Intake intake = Intake.getInstance();
+        final Shooter shooter = Shooter.getInstance();
+        final Climber climber = Climber.getInstance();
+
         // Climber Test Controls
         if (stick.getRisingEdge(9)) {
             climber.toggleClaw();
@@ -736,17 +844,20 @@ public void testPeriodic() {
 
     private void startSubsystems() {
         System.out.println("Starting Subsystems");
-        robotTracker.start();
-        drive.start();
-        intake.start();
-        hopper.start();
-        shooter.start();
-        climber.start();
-        visionManager.start();
+        RobotTracker.getInstance().start();
+        Drive.getInstance().start();
+        Intake.getInstance().start();
+        Hopper.getInstance().start();
+        Shooter.getInstance().start();
+        Climber.getInstance().start();
+        VisionManager.getInstance().start();
+        DashboardHandler.getInstance().start();
     }
 
     public void killAuto() {
         System.out.println("Killing Auto");
+        final Drive drive = Drive.getInstance();
+
         if (selectedAuto != null) {
             assert autoThread != null;
             System.out.println("2");
@@ -780,5 +891,6 @@ public void killAuto() {
     public void simulationInit() {
         ClassInformationSender.updateReflectionInformation(
                 new File(OsUtil.getUserConfigDirectory("AutoBuilder") + "/robotCodeData.json"));
+        VisionLookUpTable.getInstance().printShooterConfig();
     }
 }
diff --git a/src/main/java/frc/subsystem/AbstractSubsystem.java b/src/main/java/frc/subsystem/AbstractSubsystem.java
index 43a8c1d1..c97d0532 100644
--- a/src/main/java/frc/subsystem/AbstractSubsystem.java
+++ b/src/main/java/frc/subsystem/AbstractSubsystem.java
@@ -68,6 +68,7 @@ public void update() {
     }
 
     int lastLength = 20;
+
     @Override
     @SuppressWarnings("BusyWait")
     public void run() {
diff --git a/src/main/java/frc/subsystem/Climber.java b/src/main/java/frc/subsystem/Climber.java
index cf651513..8ce23390 100644
--- a/src/main/java/frc/subsystem/Climber.java
+++ b/src/main/java/frc/subsystem/Climber.java
@@ -83,6 +83,7 @@ public static Climber getInstance() {
 
     private boolean isPaused = false;
     private double pausedClimberSetpoint;
+    private @NotNull BrakeState pausedBrakeState = BrakeState.BRAKING;
     private ControlMode pausedClimberMode;
 
     private boolean sensorClimb = true;
@@ -90,6 +91,7 @@ public static Climber getInstance() {
     private boolean advanceStep = false;
     private boolean skipChecks = false;
     private boolean ranEndAction = false;
+    private boolean startingClimb = true;
 
     public enum ClawState {
         LATCHED, UNLATCHED
@@ -158,15 +160,16 @@ public enum ClimbState {
                 new ClimbStep(
                         (cl) -> {},
                         (cl) -> {
+                            if (cl.startingClimb) return true;
                             AHRS gyro = RobotTracker.getInstance().getGyro();
-                            if (Math.abs(gyro.getRoll()) < 15 && Math.abs(RobotTracker.getInstance().getGyroRollVelocity()) < 2) {
+                            if (Math.abs(gyro.getRoll()) < 15 && Math.abs(RobotTracker.getInstance().getGyroRollVelocity()) < 5) {
                                 return true;
                             } else {
                                 return gyro.getRoll() > 20 && gyro.getRoll() < 40 &&
                                         RobotTracker.getInstance().getGyroRollVelocity() > 0;
                             }
                         },
-                        (cl) -> {},
+                        (cl) -> cl.startingClimb = false,
                         false
                 )
         ),
@@ -177,6 +180,7 @@ public enum ClimbState {
         LOWER_ELEVATOR_ARM_TILL_PIVOT_ARM_CONTACT(
                 new ClimbStep(
                         (cl) -> {
+                            cl.setBrakeState(BrakeState.FREE);
                             cl.climberMotor.set(ControlMode.PercentOutput, -Constants.CLIMBER_MOTOR_MAX_OUTPUT);
                             cl.setClawState(ClawState.UNLATCHED);
                         },
@@ -187,6 +191,7 @@ public enum ClimbState {
 
                 new ClimbStep(
                         (cl) -> {
+                            cl.setBrakeState(BrakeState.FREE);
                             cl.climberMotor.set(ControlMode.MotionMagic, Constants.CLIMBER_GRAB_ON_FIRST_BAR_EXTENSION);
                             cl.setClawState(ClawState.UNLATCHED);
                         },
@@ -401,11 +406,30 @@ public enum ClimbState {
                         },
                         (cl) -> cl.climberMotor.getSelectedSensorPosition() - (0.3 * CLIMBER_ENCODER_TICKS_PER_INCH) < CLIMBER_GRAB_ON_NEXT_BAR_EXTENSION,
                         // TODO: Determine if we want this
-                        (cl) -> {},
+                        (cl) -> {
+                            cl.stopClimberMotor();
+                            cl.setBrakeState(BrakeState.BRAKING);
+                        },
                         true
                 )
         ),
 
+        WAIT_FOR_BRAKE_TIME(
+                new ClimbStep(
+                        (cl) -> cl.data = Timer.getFPGATimestamp(),
+                        (cl) -> Timer.getFPGATimestamp() - cl.data > 0.25,
+                        (cl) -> cl.climberMotor.set(ControlMode.PercentOutput, 0),
+                        false
+                ),
+
+                new ClimbStep(
+                        (cl) -> cl.data = Timer.getFPGATimestamp(),
+                        (cl) -> Timer.getFPGATimestamp() - cl.data > 0.25,
+                        (cl) -> cl.climberMotor.set(ControlMode.PercentOutput, 0),
+                        false
+                )
+        ),
+
         /**
          * Unlatches the pivot arm so that the robot is supported by the elevator arm.
          */
@@ -566,6 +590,8 @@ private Climber() {
      * Starts the automated climb sequence and deactivates the brake.
      */
     public synchronized void startClimb() {
+        isPaused = false;
+        otherPivotingArmMustContactByTime = Double.MAX_VALUE;
         climbState = ClimbState.START_CLIMB;
         setBrakeState(BrakeState.FREE);
     }
@@ -595,6 +621,7 @@ public synchronized void pauseClimb() {
         } else {
             pausedClimberSetpoint = climberMotor.getClosedLoopTarget();
         }
+        pausedBrakeState = getBrakeState();
         climberMotor.set(ControlMode.PercentOutput, 0);
         setBrakeState(BrakeState.BRAKING);
     }
@@ -613,19 +640,21 @@ private synchronized void stopClimberMotor() {
      */
     public synchronized void resumeClimb() {
         isPaused = false;
-        setBrakeState(BrakeState.FREE);
-        climberMotor.set(pausedClimberMode, pausedClimberSetpoint);
         otherPivotingArmMustContactByTime = Double.MAX_VALUE;
+        setBrakeState(pausedBrakeState);
+        climberMotor.set(pausedClimberMode, pausedClimberSetpoint);
     }
 
     /**
      * Sets the climber in the correct state to initiate a climb and moves the elevator arm to the up above the high bar.
      */
     public synchronized void deployClimb() {
+        timesRun = 0;
         climberMotor.set(ControlMode.MotionMagic, Constants.CLIMBER_DEPLOY_HEIGHT);
         setBrakeState(BrakeState.FREE);
         setClawState(ClawState.UNLATCHED);
         setPivotState(PivotState.INLINE);
+        startingClimb = true;
     }
 
     /**
@@ -645,6 +674,7 @@ public synchronized void advanceStep() {
      * This method will also work regardless of whether the robot is in step-by-mode or not.
      */
     public synchronized void forceAdvanceStep() {
+        if (isPaused) resumeClimb();
         advanceStep = true;
         skipChecks = true;
     }
@@ -683,13 +713,13 @@ public synchronized void update() {
 
 
         if (!isPaused) {
-            if (climberMotor.getSelectedSensorPosition() < Constants.MIN_CLIMBER_ELEVATOR_HEIGHT
-                    && climberMotor.getSelectedSensorVelocity() < 0) {
-                stopClimb();
-            } else if (climberMotor.getSelectedSensorPosition() > Constants.MAX_CLIMBER_ELEVATOR_HEIGHT
-                    && climberMotor.getSelectedSensorVelocity() > 0) {
-                stopClimb();
-            }
+//            if (climberMotor.getSelectedSensorPosition() < Constants.MIN_CLIMBER_ELEVATOR_HEIGHT
+//                    && climberMotor.getSelectedSensorVelocity() < 0) {
+//                stopClimb();
+//            } else if (climberMotor.getSelectedSensorPosition() > Constants.MAX_CLIMBER_ELEVATOR_HEIGHT
+//                    && climberMotor.getSelectedSensorVelocity() > 0) {
+//                stopClimb();
+//            }
 
             if (currentClimbStep.waitCondition.apply(this)) {
                 currentClimbStep.endAction.accept(this);
diff --git a/src/main/java/frc/subsystem/DashboardHandler.java b/src/main/java/frc/subsystem/DashboardHandler.java
index c63e18cc..00707b31 100644
--- a/src/main/java/frc/subsystem/DashboardHandler.java
+++ b/src/main/java/frc/subsystem/DashboardHandler.java
@@ -3,19 +3,15 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants;
-import frc.utility.Serializer;
 import frc.utility.Timer;
 import frc.utility.net.DashboardConnection;
 import frc.utility.net.PacketHandler;
-import frc.utility.net.SendableLog;
 import org.jetbrains.annotations.NotNull;
 import org.jetbrains.annotations.Nullable;
 
 import java.io.IOException;
 import java.net.*;
-import java.nio.charset.StandardCharsets;
 import java.util.HashMap;
-import java.util.Iterator;
 import java.util.Map;
 import java.util.concurrent.ConcurrentHashMap;
 import java.util.concurrent.locks.ReadWriteLock;
@@ -69,13 +65,14 @@ public final class DashboardHandler extends AbstractSubsystem {
 
     private DashboardHandler(int period) {
         super(period);
-
-        try {
-            receivingSocket = new DatagramSocket(WEB_DASHBOARD_PORT);
-            receivingSocket.setSoTimeout(5);
-        } catch (SocketException e) {
-            DriverStation.reportError("Could not create socket for listening for data from web dashboard", false);
-        }
+        receivingSocket = null; //Disables networking code
+
+//        try {
+//            receivingSocket = new DatagramSocket(WEB_DASHBOARD_PORT);
+//            receivingSocket.setSoTimeout(5);
+//        } catch (SocketException e) {
+//            DriverStation.reportError("Could not create socket for listening for data from web dashboard", false);
+//        }
     }
 
     public void log(@NotNull String key, @NotNull Object value) {
@@ -99,73 +96,66 @@ public void log(@NotNull String key, @NotNull Object value, boolean logToNetwork
     }
 
     public void pushLog() {
+        String json;
+        LOG_DATA_MAP_LOCK.writeLock().lock(); // Ensure that no other writes happen while we're serializing
         try {
-            String json;
-            LOG_DATA_MAP_LOCK.writeLock().lock(); // Ensure that no other writes happen while we're serializing
-            try {
-                SendableLog log = new SendableLog(LOG_DATA_MAP);
-                json = 'd' + Serializer.serializeToString(log);
-
-                for (Map.Entry<String, Object> entry : LOG_DATA_MAP_OBJECTS.entrySet()) {
-                    //@formatter:off
-                    String key = entry.getKey();
-                    Class<?> cl = entry.getValue().getClass();
-                    Object value = entry.getValue();
-                    if (cl.equals(Integer.class)) SmartDashboard.putNumber(key, (int) value);
-                    else if (cl.equals(Double.class)) SmartDashboard.putNumber(key, (double) value);
-                    else if (cl.equals(Short.class)) SmartDashboard.putNumber(key, (short) value);
-                    else if (cl.equals(Long.class)) SmartDashboard.putNumber(key, (long) value);
-                    else if (cl.equals(Float.class)) SmartDashboard.putNumber(key, (float) value);
-                    else if (cl.equals(Byte.class)) SmartDashboard.putNumber(key, (byte) value);
-                    else if (cl.equals(Boolean.class)) SmartDashboard.putBoolean(key, (boolean) value);
-                    else if (cl.equals(String.class)) SmartDashboard.putString(key, (String) value);
-                    else if (cl.equals(Double[].class)) SmartDashboard.putNumberArray(key, (Double[]) value);
-                    else if (cl.equals(Boolean[].class)) SmartDashboard.putBooleanArray(key, (Boolean[]) value);
-                    else if (cl.equals(String[].class)) SmartDashboard.putStringArray(key, (String[]) value);
-                    else SmartDashboard.putString(key, value.toString());
-                    //@formatter:on
-                }
-            } finally {
-                LOG_DATA_MAP_LOCK.writeLock().unlock();
-            }
-
-            synchronized (dashboardConnections) {
-                Iterator<DashboardConnection> iterator = dashboardConnections.values().iterator();
-                while (iterator.hasNext()) {
-                    DashboardConnection entry = iterator.next();
-                    if (Timer.getFPGATimestamp() > entry.timeoutTime) {
-                        // If we haven't received a keepalive in a while, remove the connection
-                        entry.close();
-                        iterator.remove();
-                        System.out.println("closing packet: " + entry.datagramSocket.getInetAddress() + " timed out");
-                    } else {
-                        byte[] bytes = json.getBytes(StandardCharsets.ISO_8859_1);
-                        try {
-                            DatagramPacket packet = new DatagramPacket(bytes, bytes.length);
-
-                            entry.datagramSocket.send(packet);
-                            System.out.println(
-                                    "sending packet of " + packet.getLength() + " length to " + entry.datagramSocket.getInetAddress());
-                        } catch (PortUnreachableException e) {
-                            System.out.println("closing packet: " + entry.datagramSocket.getInetAddress() + " got an error");
-                            entry.close();
-                            iterator.remove(); // Remove the connection if it's unreachable
-                        }
-                    }
-                }
-            }
-        } catch (IOException e) {
-            if (Timer.getFPGATimestamp() > nextAllowedErrorTime) { // Don't spam the driver station
-                DriverStation.reportError("Could not send data to web dashboard", false);
-                nextAllowedErrorTime = Timer.getFPGATimestamp() + 5;
+            //SendableLog log = new SendableLog(LOG_DATA_MAP);
+            //json = 'd' + Serializer.serializeToString(log);
+
+            for (Map.Entry<String, Object> entry : LOG_DATA_MAP_OBJECTS.entrySet()) {
+                //@formatter:off
+                String key = entry.getKey();
+                Class<?> cl = entry.getValue().getClass();
+                Object value = entry.getValue();
+                if (cl.equals(Integer.class)) SmartDashboard.putNumber(key, (int) value);
+                else if (cl.equals(Double.class)) SmartDashboard.putNumber(key, (double) value);
+                else if (cl.equals(Short.class)) SmartDashboard.putNumber(key, (short) value);
+                else if (cl.equals(Long.class)) SmartDashboard.putNumber(key, (long) value);
+                else if (cl.equals(Float.class)) SmartDashboard.putNumber(key, (float) value);
+                else if (cl.equals(Byte.class)) SmartDashboard.putNumber(key, (byte) value);
+                else if (cl.equals(Boolean.class)) SmartDashboard.putBoolean(key, (boolean) value);
+                else if (cl.equals(String.class)) SmartDashboard.putString(key, (String) value);
+                else if (cl.equals(Double[].class)) SmartDashboard.putNumberArray(key, (Double[]) value);
+                else if (cl.equals(Boolean[].class)) SmartDashboard.putBooleanArray(key, (Boolean[]) value);
+                else if (cl.equals(String[].class)) SmartDashboard.putStringArray(key, (String[]) value);
+                else SmartDashboard.putString(key, value.toString());
+                //@formatter:on
             }
+        } finally {
+            LOG_DATA_MAP_LOCK.writeLock().unlock();
         }
+
+//            synchronized (dashboardConnections) {
+//                Iterator<DashboardConnection> iterator = dashboardConnections.values().iterator();
+//                while (iterator.hasNext()) {
+//                    DashboardConnection entry = iterator.next();
+//                    if (Timer.getFPGATimestamp() > entry.timeoutTime) {
+//                        // If we haven't received a keepalive in a while, remove the connection
+//                        entry.close();
+//                        iterator.remove();
+//                        System.out.println("closing packet: " + entry.datagramSocket.getInetAddress() + " timed out");
+//                    } else {
+//                        byte[] bytes = json.getBytes(StandardCharsets.ISO_8859_1);
+//                        try {
+//                            DatagramPacket packet = new DatagramPacket(bytes, bytes.length);
+//
+//                            entry.datagramSocket.send(packet);
+//                            System.out.println(
+//                                    "sending packet of " + packet.getLength() + " length to " + entry.datagramSocket.getInetAddress());
+//                        } catch (PortUnreachableException e) {
+//                            System.out.println("closing packet: " + entry.datagramSocket.getInetAddress() + " got an error");
+//                            entry.close();
+//                            iterator.remove(); // Remove the connection if it's unreachable
+//                        }
+//                    }
+//                }
+//            }
     }
 
     byte[] receivedBytes = new byte[65535];
 
     private void handleDashboardPackets() {
-        if (receivingSocket == null) return;
+        if (receivingSocket == null || true) return;
 
         for (int i = 0; i < 100; i++) { // Receive up to 100 packets at a time
             try {
@@ -212,4 +202,4 @@ public void logData() {
     public void close() throws Exception {
 
     }
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/subsystem/Drive.java b/src/main/java/frc/subsystem/Drive.java
index 159b81e8..abad448f 100644
--- a/src/main/java/frc/subsystem/Drive.java
+++ b/src/main/java/frc/subsystem/Drive.java
@@ -34,6 +34,7 @@
 import frc.utility.wpimodified.HolonomicDriveController;
 import org.jetbrains.annotations.Contract;
 import org.jetbrains.annotations.NotNull;
+import org.jetbrains.annotations.Nullable;
 
 import java.util.concurrent.locks.Lock;
 import java.util.concurrent.locks.ReentrantLock;
@@ -53,16 +54,21 @@ public final class Drive extends AbstractSubsystem {
     final @NotNull NetworkTableEntry turnMaxAcceleration = SmartDashboard.getEntry("TurnMaxAcceleration");
 
     public void resetAuto() {
-        ProfiledPIDController autoTurnPIDController
-                = new ProfiledPIDController(8, 0, 0.01, new TrapezoidProfile.Constraints(4, 4));
-        autoTurnPIDController.enableContinuousInput(-Math.PI, Math.PI);
-        autoTurnPIDController.setTolerance(Math.toRadians(10));
-        
-        swerveAutoController.setTolerance(new Pose2d(0.5, 0.5, Rotation2d.fromDegrees(10))); //TODO: Tune
-        swerveAutoController = new HolonomicDriveController(
-                new PIDController(3, 0, 0),
-                new PIDController(3, 0, 0),
-                autoTurnPIDController);
+        swerveAutoControllerLock.lock();
+        try {
+            ProfiledPIDController autoTurnPIDController
+                    = new ProfiledPIDController(8, 0, 0.01, new TrapezoidProfile.Constraints(4, 4));
+            autoTurnPIDController.enableContinuousInput(-Math.PI, Math.PI);
+            autoTurnPIDController.setTolerance(Math.toRadians(10));
+
+            swerveAutoController = new HolonomicDriveController(
+                    new PIDController(3, 0, 0),
+                    new PIDController(3, 0, 0),
+                    autoTurnPIDController);
+            swerveAutoController.setTolerance(new Pose2d(0.5, 0.5, Rotation2d.fromDegrees(10))); //TODO: Tune
+        } finally {
+            swerveAutoControllerLock.unlock();
+        }
     }
 
     public enum DriveState {
@@ -77,11 +83,10 @@ public enum DriveState {
         return INSTANCE;
     }
 
-    private final @NotNull ProfiledPIDController turnPID;
+    private final @NotNull PIDController turnPID;
 
     {
-        turnPID = new ProfiledPIDController(DEFAULT_TURN_P, DEFAULT_TURN_I, DEFAULT_TURN_D,
-                new TrapezoidProfile.Constraints(DEFAULT_TURN_MAX_VELOCITY, DEFAULT_TURN_MAX_ACCELERATION)); //P=1.0
+        turnPID = new PIDController(DEFAULT_TURN_P, DEFAULT_TURN_I, DEFAULT_TURN_D); //P=1.0
         // OR 0.8
         turnPID.enableContinuousInput(-Math.PI, Math.PI);
     }
@@ -98,7 +103,7 @@ public enum DriveState {
 
     private boolean isAiming = false;
 
-    private @NotNull final SwerveDriveKinematics swerveKinematics = new SwerveDriveKinematics(
+    private @NotNull static final SwerveDriveKinematics SWERVE_DRIVE_KINEMATICS = new SwerveDriveKinematics(
             Constants.SWERVE_LEFT_FRONT_LOCATION,
             Constants.SWERVE_LEFT_BACK_LOCATION,
             Constants.SWERVE_RIGHT_FRONT_LOCATION,
@@ -209,16 +214,16 @@ private Drive() {
         turnD.addListener(event -> turnPID.setD(event.getEntry().getDouble(Constants.DEFAULT_TURN_D)),
                 EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
 
-        turnMaxVelocity.addListener(event -> turnPID.setConstraints(
-                        new TrapezoidProfile.Constraints(turnMaxVelocity.getDouble(DEFAULT_TURN_MAX_VELOCITY),
-                                turnMaxAcceleration.getDouble(6))),
-                EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
-
-        turnMaxAcceleration.addListener(
-                event -> turnPID.setConstraints(
-                        new TrapezoidProfile.Constraints(turnMaxVelocity.getDouble(DEFAULT_TURN_MAX_ACCELERATION),
-                                turnMaxAcceleration.getDouble(6))),
-                EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+//        turnMaxVelocity.addListener(event -> turnPID.setConstraints(
+//                        new TrapezoidProfile.Constraints(turnMaxVelocity.getDouble(DEFAULT_TURN_MAX_VELOCITY),
+//                                turnMaxAcceleration.getDouble(6))),
+//                EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+//
+//        turnMaxAcceleration.addListener(
+//                event -> turnPID.setConstraints(
+//                        new TrapezoidProfile.Constraints(turnMaxVelocity.getDouble(DEFAULT_TURN_MAX_ACCELERATION),
+//                                turnMaxAcceleration.getDouble(6))),
+//                EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
 
 
         useFieldRelative = true;
@@ -278,8 +283,8 @@ private double getSwerveDriveVelocity(int motorNum) {
         return swerveDriveMotors[motorNum].getSelectedSensorVelocity() * Constants.FALCON_ENCODER_TICKS_PER_100_MS_TO_RPM * Constants.SWERVE_DRIVE_MOTOR_REDUCTION;
     }
 
-    public @NotNull SwerveDriveKinematics getSwerveDriveKinematics() {
-        return swerveKinematics;
+    public static @NotNull SwerveDriveKinematics getSwerveDriveKinematics() {
+        return SWERVE_DRIVE_KINEMATICS;
     }
 
     public synchronized void setDriveState(@NotNull DriveState driveState) {
@@ -290,7 +295,7 @@ public void setTeleop() {
         setDriveState(DriveState.TELEOP);
     }
 
-    synchronized public SwerveModuleState @NotNull [] getSwerveModuleStates() {
+    public @NotNull SwerveModuleState[] getSwerveModuleStates() {
         SwerveModuleState[] swerveModuleState = new SwerveModuleState[4];
         for (int i = 0; i < 4; i++) {
             SwerveModuleState moduleState = new SwerveModuleState(
@@ -351,7 +356,7 @@ public void swerveDrive(ChassisSpeeds chassisSpeeds) {
         SmartDashboard.putNumber("Drive Command Y Velocity", chassisSpeeds.vyMetersPerSecond);
         SmartDashboard.putNumber("Drive Command Rotation", chassisSpeeds.omegaRadiansPerSecond);
 
-        SwerveModuleState[] moduleStates = swerveKinematics.toSwerveModuleStates(chassisSpeeds);
+        SwerveModuleState[] moduleStates = SWERVE_DRIVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds);
 
         boolean rotate = chassisSpeeds.vxMetersPerSecond != 0 ||
                 chassisSpeeds.vyMetersPerSecond != 0 ||
@@ -424,7 +429,7 @@ public double getAngleDiff(double targetAngle, double currentAngle) {
     @Contract(mutates = "param")
     @NotNull ChassisSpeeds limitAcceleration(@NotNull ChassisSpeeds commandedVelocity) {
         double dt;
-        if ((Timer.getFPGATimestamp() - lastLoopTime) > ((double) Constants.DRIVE_PERIOD / 1000) * 4) {
+        if ((Timer.getFPGATimestamp() - lastLoopTime) > ((double) Constants.DRIVE_PERIOD / 1000) * 20) {
             // If the dt is a lot greater than our nominal dt reset the acceleration limiting
             // (ex. we've been disabled for a while)
             lastRequestedVelocity = RobotTracker.getInstance().getLatencyCompedChassisSpeeds();
@@ -520,7 +525,8 @@ public double getSpeedSquared() {
 
     double autoStartTime;
 
-    private @NotNull HolonomicDriveController swerveAutoController;
+    private final ReentrantLock swerveAutoControllerLock = new ReentrantLock();
+    private @Nullable HolonomicDriveController swerveAutoController;
 
     public void setAutoPath(Trajectory trajectory) {
         currentAutoTrajectoryLock.lock();
@@ -549,6 +555,7 @@ public void setAutoAiming(boolean autoAiming) {
     }
 
 
+    double nextAllowedPrintError = 0;
     private void updateRamsete() {
         currentAutoTrajectoryLock.lock();
         try {
@@ -559,15 +566,23 @@ private void updateRamsete() {
                 targetHeading = VisionManager.getInstance().getAngleToTarget();
             }
 
-            ChassisSpeeds adjustedSpeeds = swerveAutoController.calculate(
-                    RobotTracker.getInstance().getLastEstimatedPoseMeters(),
-                    goal,
-                    targetHeading);
-
-            swerveDrive(adjustedSpeeds);
-            if (swerveAutoController.atReference() && (Timer.getFPGATimestamp() - autoStartTime) >= currentAutoTrajectory.getTotalTimeSeconds()) {
-                setDriveState(DriveState.DONE);
-                stopMovement();
+            if (swerveAutoController == null) resetAuto();
+            if (swerveAutoController == null) return;
+            try {
+                ChassisSpeeds adjustedSpeeds = swerveAutoController.calculate(
+                        RobotTracker.getInstance().getLastEstimatedPoseMeters(),
+                        goal,
+                        targetHeading);
+                swerveDrive(adjustedSpeeds);
+                if (swerveAutoController.atReference() && (Timer.getFPGATimestamp() - autoStartTime) >= currentAutoTrajectory.getTotalTimeSeconds()) {
+                    setDriveState(DriveState.DONE);
+                    stopMovement();
+                }
+            } catch (NullPointerException exception) {
+                if (Timer.getFPGATimestamp() > nextAllowedPrintError) {
+                    exception.printStackTrace();
+                    nextAllowedPrintError = Timer.getFPGATimestamp() + 2;
+                }
             }
         } finally {
             currentAutoTrajectoryLock.unlock();
@@ -636,7 +651,12 @@ public void centerOntoBall(ControllerDriveInputs inputs, boolean fieldRelativeEn
     }
 
     public void setAutoRotation(@NotNull Rotation2d rotation) {
-        autoTargetHeading = rotation;
+        currentAutoTrajectoryLock.lock();
+        try {
+            autoTargetHeading = rotation;
+        } finally {
+            currentAutoTrajectoryLock.unlock();
+        }
         System.out.println("new rotation" + rotation.getDegrees());
     }
 
@@ -686,21 +706,24 @@ synchronized public boolean isAiming() {
         return isAiming;
     }
 
-    public synchronized void setRotation(Rotation2d angle) {
+    public void setRotation(Rotation2d angle) {
         wantedHeading = angle;
         driveState = DriveState.TURN;
         rotateAuto = true;
         isAiming = !isTurningDone();
     }
 
-    public synchronized void setRotation(double angle) {
+    public void setRotation(double angle) {
         setRotation(Rotation2d.fromDegrees(angle));
     }
 
 
-    public synchronized boolean isTurningDone() {
-        double error = wantedHeading.minus(RobotTracker.getInstance().getGyroAngle()).getDegrees();
-        return (Math.abs(error) < Constants.MAX_TURN_ERROR);
+    public boolean isTurningDone() {
+        Rotation2d currentHeading = RobotTracker.getInstance().getGyroAngle();
+        synchronized (this) {
+            double error = wantedHeading.minus(currentHeading).getDegrees();
+            return (Math.abs(error) < Constants.MAX_TURN_ERROR);
+        }
     }
 
     double turnMinSpeed = 0;
@@ -746,14 +769,14 @@ public void updateTurn(ControllerDriveInputs controllerDriveInputs, State goal,
             if (driveState != DriveState.TURN) setDriveState(DriveState.TELEOP);
         }
 
-        if (Timer.getFPGATimestamp() - 0.2 > lastTurnUpdate) {
-            turnPID.reset(RobotTracker.getInstance().getGyroAngle().getRadians(),
-                    RobotTracker.getInstance().getLatencyCompedChassisSpeeds().omegaRadiansPerSecond);
-        }
 
+        turnPID.setSetpoint(goal.position);
 
+        if (Timer.getFPGATimestamp() - 0.2 > lastTurnUpdate || turnPID.getPositionError() > Math.toRadians(7)) {
+            turnPID.reset();
+        }
         lastTurnUpdate = Timer.getFPGATimestamp();
-        double pidDeltaSpeed = turnPID.calculate(RobotTracker.getInstance().getGyroAngle().getRadians(), goal);
+        double pidDeltaSpeed = turnPID.calculate(RobotTracker.getInstance().getGyroAngle().getRadians()) + goal.velocity;
 
 //        System.out.println(
 //                "turn error: " + Math.toDegrees(turnPID.getPositionError()) + " delta speed: " + Math.toDegrees(pidDeltaSpeed));
@@ -918,7 +941,7 @@ public void turnToAngle(double degrees) throws InterruptedException {
         System.out.println("Turning to " + degrees);
         setRotation(degrees);
         while (!isTurningDone()) {
-            Thread.onSpinWait();
+            Thread.sleep(20);
         }
     }
 
diff --git a/src/main/java/frc/subsystem/Hopper.java b/src/main/java/frc/subsystem/Hopper.java
index b40198bc..242eec89 100644
--- a/src/main/java/frc/subsystem/Hopper.java
+++ b/src/main/java/frc/subsystem/Hopper.java
@@ -1,42 +1,31 @@
 package frc.subsystem;
 
+import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 import com.revrobotics.CANSparkMaxLowLevel.PeriodicFrame;
-import com.revrobotics.ColorMatch;
-import com.revrobotics.ColorSensorV3;
-import edu.wpi.first.wpilibj.I2C;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.util.Color;
+import com.revrobotics.RelativeEncoder;
 import frc.robot.Constants;
+import frc.robot.Robot;
+import frc.subsystem.Intake.IntakeSolState;
+import frc.utility.Limelight;
 import frc.utility.OrangeUtility;
+import frc.utility.Timer;
 import frc.utility.controllers.LazyCANSparkMax;
+import org.jetbrains.annotations.Contract;
 import org.jetbrains.annotations.NotNull;
 
 import static frc.robot.Constants.HOPPER_CURRENT_LIMIT;
 
 public final class Hopper extends AbstractSubsystem {
-    public static @NotNull Hopper INSTANCE = new Hopper();
-    private final @NotNull LazyCANSparkMax hopperMotor;
-    private final @NotNull ColorSensorV3 topBall;
-    //private final ColorSensorV3 bottomBall = new ColorSensorV3(Port.kMXP);
-
-    private boolean topBallDetected = false;
-    private boolean bottomBallDetected = false;
-
 
-    /**
-     * A Rev Color Match object is used to register and detect known colors. This can be calibrated ahead of time or during
-     * operation.
-     * <p>
-     * This object uses a simple euclidian distance to estimate the closest match with given confidence range.
-     */
-    private final ColorMatch colorMatcher = new ColorMatch();
-
-    /**
-     * Note: Any example colors should be calibrated as the user needs, these are here as a basic example.
-     */
-    private final Color blueTarget = new Color(0.184, 0.427, 0.39);
-    private final Color redTarget = new Color(0.38, 0.41, 0.2);
+    private static @NotNull Hopper INSTANCE = new Hopper();
+    private final @NotNull LazyCANSparkMax hopperMotor;
+    private RelativeEncoder outtakeWheelsQuadrature;
+    private LazyCANSparkMax outtakeWheels;
+    private double lastDetectionTime;
+    private boolean disableEject = false;
+    private final Limelight intakeLimelight = Limelight.getInstance(Constants.INTAKE_LIMELIGHT_NAME);
 
     public static Hopper getInstance() {
         return INSTANCE;
@@ -48,6 +37,27 @@ public enum HopperState {
 
     HopperState wantedHopperState = HopperState.OFF;
 
+    /**
+     * Outtake can either be OFF or INTAKE, or AUTO_EJECT
+     */
+    public enum OuttakeState {
+        OFF,
+        INTAKE,
+        AUTO_EJECT,
+        MANUAL_EJECT
+    }
+
+    private OuttakeState outtakeState = OuttakeState.OFF;
+
+    public enum BallColor {
+        RED,
+        BLUE,
+        NO_BALL
+    }
+
+    BallColor opposingAllianceColor = BallColor.BLUE;
+    BallColor friendlyAllianceColor = BallColor.RED;
+
     private Hopper() {
         super(Constants.HOPPER_PERIOD, 5);
         hopperMotor = new LazyCANSparkMax(Constants.HOPPER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless);
@@ -58,60 +68,135 @@ private Hopper() {
         hopperMotor.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 500);
         hopperMotor.setControlFramePeriodMs(25);
 
-        colorMatcher.addColorMatch(blueTarget);
-        colorMatcher.addColorMatch(redTarget);
-        topBall = new ColorSensorV3(I2C.Port.kMXP);
+        outtakeWheels = new LazyCANSparkMax(Constants.OUTTAKE_CAN_ID, MotorType.kBrushless);
+        outtakeWheels.setIdleMode(IdleMode.kCoast);
+        outtakeWheels.setInverted(true);
+        outtakeWheels.setSmartCurrentLimit(Constants.OUTTAKE_CURRENT_LIMIT);
+        outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus0, 100);
+        outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus1, 200);
+        outtakeWheels.setPeriodicFramePeriod(PeriodicFrame.kStatus2, 25);
+        outtakeWheels.setControlFramePeriodMs(25);
+
+        outtakeWheelsQuadrature = outtakeWheels.getEncoder();
+        outtakeWheelsQuadrature.setPositionConversionFactor(Constants.OUTTAKE_REDUCTION);
+        outtakeWheelsQuadrature.setMeasurementPeriod(Constants.OUTTAKE_MEASUREMENT_PERIOD_MS);
+
+        outtakeWheels.disableVoltageCompensation();
+        outtakeWheels.burnFlash();
     }
 
-    public void checkBallColor() {
-        Color detectedColorTop = topBall.getColor();
-        //Color detectedColorBottom = bottomBall.getColor();
+    /**
+     * Uses Sendable Chooser to decide Alliance Color
+     */
+    private void updateAllianceColor() {
+        if (Robot.sideChooser.getSelected().equals(Robot.BLUE)) {
+            opposingAllianceColor = BallColor.RED;
+            friendlyAllianceColor = BallColor.BLUE;
 
-        logData("Top Ball Red", detectedColorTop.red);
-        logData("Top Ball Green", detectedColorTop.green);
-        logData("Top Ball Blue", detectedColorTop.blue);
-        logData("Top Confidence", colorMatcher.matchClosestColor(detectedColorTop).confidence);
+            // Will detect opposite color balls and outtake when it sees them
+            intakeLimelight.setPipeline(1);
+        } else {
+            opposingAllianceColor = BallColor.BLUE;
+            friendlyAllianceColor = BallColor.RED;
 
-//        logData("Bottom Red", detectedColorBottom.red);
-//        logData("Bottom Green", detectedColorBottom.green);
-//        logData("Bottom Blue", detectedColorBottom.blue);
-//        logData("Bottom Confidence", colorMatcher.matchClosestColor(detectedColorBottom).confidence);
+            // Will detect same colored balls and intake when it sees them
+            intakeLimelight.setPipeline(0);
+        }
+    }
+
+    /**
+     * Toggles disableEject
+     */
+    public void toggleEjectOverride() {
+        disableEject = !disableEject;
+    }
+
+    /**
+     * Toggles disableEject
+     */
+    public void setEjectOverride(boolean ejectOverride) {
+        disableEject = ejectOverride;
+    }
+
+    /**
+     * Updates state of outtake based on color sensor and intake direction
+     */
+    private void updateOuttakeState() {
+        Intake intake = Intake.getInstance();
 
-        if (colorMatcher.matchClosestColor(detectedColorTop).color == redTarget) {
-            logData("Top Ball Color", "Red");
-            topBallDetected = true;
-        } else if (colorMatcher.matchClosestColor(detectedColorTop).color == blueTarget) {
-            logData("Top Ball Color", "Blue");
-            topBallDetected = true;
+        if (wantedHopperState == HopperState.REVERSE) {
+            outtakeState = intake.getIntakeSolState() == IntakeSolState.OPEN ? OuttakeState.MANUAL_EJECT : OuttakeState.OFF;
+            return;
+        }
+        if (wantedHopperState == HopperState.OFF) {
+            outtakeState = OuttakeState.OFF;
+            return;
+        }
+
+        if (getBallColor() == opposingAllianceColor
+                && intake.getIntakeSolState() == IntakeSolState.OPEN
+                && !disableEject) { // If disable eject is on, it will not outtake
+            lastDetectionTime = Timer.getFPGATimestamp();
+            outtakeState = OuttakeState.AUTO_EJECT;
+        } else if (Timer.getFPGATimestamp() - lastDetectionTime < Constants.OUTTAKE_RUN_PERIOD
+                && intake.getIntakeSolState() == IntakeSolState.OPEN) {
+            // Ensure that we keep outtaking for a minimum amount of time
+            outtakeState = OuttakeState.AUTO_EJECT;
         } else {
-            logData("Top Ball Color", "No Ball Detected");
-            topBallDetected = false;
+            outtakeState = OuttakeState.INTAKE;
         }
+    }
 
-//        if (colorMatcher.matchClosestColor(detectedColorBottom).color == redTarget) {
-//            logData("Bottom Ball Color", "Red");
-//            bottomBallDetected = true;
-//        } else if (colorMatcher.matchClosestColor(detectedColorBottom).color == blueTarget) {
-//            logData("Bottom Ball Color", "Blue");
-//            bottomBallDetected = true;
-//        } else {
-//            logData("Bottom Ball Color", "No Ball Detected");
-//            bottomBallDetected = false;
-//        }
-    }
-
-    public void numberOfBalls() {
-        if (topBallDetected && bottomBallDetected) {
-            logData("Number of Balls", 2);
-        } else if (bottomBallDetected || topBallDetected) {
-            logData("Number of Balls", 1);
+    /**
+     * Sets the percent outtake between 1 and -1
+     */
+    private void setOuttakePercentOutput(double percentOutput) {
+        outtakeWheels.set(percentOutput);
+        //outtakeWheels.getPIDController().setReference(9 * percentOutput, ControlType.kVoltage);
+    }
+
+    public OuttakeState getOuttakeState() {
+        return outtakeState;
+    }
+
+    @Contract(pure = true)
+    public BallColor getBallColor() {
+        @NotNull BallColor currentBallColor;
+
+        if (intakeLimelight.getVerticalOffset() < Constants.OUTTAKE_VERTICAL_OFFSET_THRESHOLD) {
+            currentBallColor = opposingAllianceColor;
         } else {
-            logData("Number of Balls", 0);
+            currentBallColor = BallColor.NO_BALL;
         }
+
+        return currentBallColor;
     }
 
-    public void setHopperState(HopperState hopperState) {
-        switch (hopperState) {
+    @Override
+    public void update() {
+        updateAllianceColor();
+        updateOuttakeState();
+
+        // Outtake motor control
+
+        switch (outtakeState) {
+            case OFF:
+                setOuttakePercentOutput(0);
+                break;
+            case AUTO_EJECT:
+                setOuttakePercentOutput(Constants.OUTTAKE_AUTO_EJECTION_SPEED);
+                break;
+            case INTAKE:
+                setOuttakePercentOutput(Constants.OUTTAKE_SPEED);
+                break;
+            case MANUAL_EJECT:
+                setOuttakePercentOutput(Constants.OUTTAKE_MANUAL_EJECTION_SPEED);
+                break;
+        }
+
+
+        // Hopper Motor Control
+        switch (wantedHopperState) {
             case ON:
                 hopperMotor.set(Constants.HOPPER_SPEED);
                 Shooter.getInstance().runFeederWheelReversed = true;
@@ -131,14 +216,7 @@ public void setHopperState(HopperState hopperState) {
         }
     }
 
-    @Override
-    public void update() {
-        setHopperState(wantedHopperState);
-        checkBallColor();
-        numberOfBalls();
-    }
-
-    public void setWantedHopperState(HopperState hopperState) {
+    public void setHopperState(HopperState hopperState) {
         wantedHopperState = hopperState;
     }
 
@@ -147,11 +225,30 @@ public void selfTest() {
         setHopperState(HopperState.ON);
         OrangeUtility.sleep(5000);
         setHopperState(HopperState.OFF);
+        OrangeUtility.sleep(5000);
+
+        setOuttakePercentOutput(-Constants.INTAKE_EJECTION_SPEED);
+        System.out.println("Ejecting");
+        OrangeUtility.sleep(Constants.TEST_TIME_MS);
+
+        setOuttakePercentOutput(Constants.INTAKE_EJECTION_SPEED);
+        System.out.println("Intaking");
+        OrangeUtility.sleep(Constants.TEST_TIME_MS);
+
+        System.out.println("Test Finished");
+        setOuttakePercentOutput(0);
     }
 
     @Override
     public void logData() {
-        SmartDashboard.putNumber("Hopper Motor Current", hopperMotor.getOutputCurrent());
+        logData("Hopper Motor Current", hopperMotor.getOutputCurrent());
+        logData("Outtake State", outtakeState);
+        logData("Outtake SetVoltage", outtakeWheels.getSetpoint());
+        logData("Outtake Velocity", outtakeWheelsQuadrature.getVelocity());
+        logData("Outtake Current", outtakeWheels.getOutputCurrent());
+        logData("Current Ball Color", getBallColor());
+        logData("Eject Disabled", disableEject);
+        logData("Hopper State", wantedHopperState);
     }
 
     @Override
diff --git a/src/main/java/frc/subsystem/Intake.java b/src/main/java/frc/subsystem/Intake.java
index cc91e51b..6b396cc8 100644
--- a/src/main/java/frc/subsystem/Intake.java
+++ b/src/main/java/frc/subsystem/Intake.java
@@ -5,6 +5,7 @@
 import edu.wpi.first.wpilibj.Solenoid;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants;
+import frc.subsystem.Hopper.OuttakeState;
 import frc.utility.OrangeUtility;
 import frc.utility.Timer;
 import frc.utility.controllers.LazyCANSparkMax;
@@ -50,6 +51,7 @@ public void logData() {
         SmartDashboard.putNumber("Intake Motor Speed: ", intakeMotor.get());
         SmartDashboard.putNumber("Intake Current", intakeMotor.getOutputCurrent());
         SmartDashboard.putBoolean("Intake Solenoid State: ", intakeSol.get());
+        logData("Wanted Intake State", wantedIntakeState);
     }
 
 
@@ -90,7 +92,7 @@ public enum IntakeState {
         INTAKE, EJECT, OFF
     }
 
-    IntakeState wantedIntakeState = IntakeState.OFF;
+    private IntakeState wantedIntakeState = IntakeState.OFF;
 
     public synchronized void setWantedIntakeState(IntakeState intakeState) {
         wantedIntakeState = intakeState;
@@ -99,11 +101,15 @@ public synchronized void setWantedIntakeState(IntakeState intakeState) {
     private void setIntakeState(IntakeState intakeState) {
         switch (intakeState) {
             case INTAKE:
-                intakeMotor.set(Constants.INTAKE_MOTOR_SPEED);
+                if (Hopper.getInstance().getOuttakeState() == OuttakeState.AUTO_EJECT) {
+                    intakeMotor.set(Constants.INTAKE_EJECTION_SPEED);
+                } else {
+                    intakeMotor.set(Constants.INTAKE_SPEED);
+                }
                 break;
 
             case EJECT:
-                intakeMotor.set(-Constants.INTAKE_MOTOR_SPEED);
+                intakeMotor.set(-Constants.INTAKE_SPEED);
                 break;
 
             case OFF:
diff --git a/src/main/java/frc/subsystem/RobotTracker.java b/src/main/java/frc/subsystem/RobotTracker.java
index 19909a21..866fe8d9 100644
--- a/src/main/java/frc/subsystem/RobotTracker.java
+++ b/src/main/java/frc/subsystem/RobotTracker.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.util.WPIUtilJNI;
@@ -28,8 +29,6 @@ public final class RobotTracker extends AbstractSubsystem {
 
     private static @NotNull RobotTracker instance = new RobotTracker();
 
-    private final Drive drive = Drive.getInstance();
-
     public static @NotNull RobotTracker getInstance() {
         return RobotTracker.instance;
     }
@@ -37,7 +36,6 @@ public final class RobotTracker extends AbstractSubsystem {
     private @NotNull Pose2d latestEstimatedPose = new Pose2d();
     private @NotNull ChassisSpeeds latestChassisSpeeds = new ChassisSpeeds();
 
-    private @NotNull Pose2d latencyCompensatedPose = new Pose2d();
     private @NotNull ChassisSpeeds latencyCompensatedChassisSpeeds = new ChassisSpeeds();
 
     private final SwerveDriveOdometry swerveDriveOdometry;
@@ -51,6 +49,8 @@ public final class RobotTracker extends AbstractSubsystem {
     private double maxGyroRoll = 0;
     private double minGyroRoll = 0;
 
+    private final @NotNull SwerveDriveKinematics swerveDriveKinematics = Drive.getSwerveDriveKinematics();
+
     private final ReentrantReadWriteLock lock = new ReentrantReadWriteLock();
 
     private Translation2d acceleration = new Translation2d();
@@ -71,7 +71,7 @@ private RobotTracker() {
 //                (Constants.ROBOT_TRACKER_PERIOD * 2) / 1000.0d); //Only update every other tick
 
         swerveDriveOdometry = new SwerveDriveOdometry(
-                drive.getSwerveDriveKinematics(),
+                swerveDriveKinematics,
                 gyroSensor.getRotation2d(),
                 new Pose2d()
 
@@ -119,27 +119,6 @@ public synchronized Rotation2d getAngle() {
         return latestEstimatedPose.getRotation();
     }
 
-    /**
-     * Resets the position on the field to 0,0 with a rotation of 0 degrees
-     */
-    synchronized public void resetOdometry() {
-        lock.writeLock().lock();
-        try {
-            Rotation2d rawGyroSensor = gyroSensor.getRotation2d();
-            swerveDriveOdometry.resetPosition(new Pose2d(), rawGyroSensor);
-            gyroOffset = latestEstimatedPose.getRotation().minus(rawGyroSensor);
-            latencyCompensatedPose = latestEstimatedPose;
-
-            latestChassisSpeeds = getRotatedSpeeds(
-                    drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()),
-                    getGyroAngle());
-
-            latencyCompensatedChassisSpeeds = latestChassisSpeeds;
-        } finally {
-            lock.writeLock().unlock();
-        }
-    }
-
     private final List<Map.Entry<Double, Rotation2d>> previousGyroRotations = new ArrayList<>(102); // 1s of data at 10ms per
 
     double currentOdometryTime = -1;
@@ -161,6 +140,7 @@ private void kalmanFilterFailed() {
      */
     @Override
     public void update() {
+        final Drive drive = Drive.getInstance();
         double time = WPIUtilJNI.now() * 1.0e-6; // seconds
 
         Rotation2d rawGyroSensor = gyroSensor.getRotation2d();
@@ -173,15 +153,15 @@ public void update() {
         }
 
         if (updateNextTick) {
-            updateOdometry(time, rawGyroSensor, drive.getSwerveModuleStates());
+            SwerveModuleState[] swerveModuleStates = drive.getSwerveModuleStates();
+            updateOdometry(time, rawGyroSensor, swerveModuleStates);
             lock.writeLock().lock();
             try {
                 latestEstimatedPose = swerveDriveOdometry.getPoseMeters();
                 //gyroOffset = latestEstimatedPose.getRotation().minus(rawGyroSensor);
-                latencyCompensatedPose = latestEstimatedPose;
 
                 latestChassisSpeeds = getRotatedSpeeds(
-                        drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()),
+                        swerveDriveKinematics.toChassisSpeeds(swerveModuleStates),
                         getGyroAngle());
 
                 latencyCompensatedChassisSpeeds = latestChassisSpeeds;
@@ -374,25 +354,25 @@ private void updateOdometry(double currentTimeSeconds, Rotation2d gyroAngle, Swe
      * @param pose The position on the field that your robot is at.
      */
     public void resetPosition(Pose2d pose) {
-        resetPosition(pose, gyroSensor.getRotation2d(), true);
+        resetPosition(pose, gyroSensor.getRotation2d());
     }
 
     public void resetPosition(Translation2d pose) {
-        resetPosition(new Pose2d(pose, getGyroAngle()), gyroSensor.getRotation2d(), false);
+        resetPosition(new Pose2d(pose, getGyroAngle()), gyroSensor.getRotation2d());
     }
 
 
-    public void resetPosition(@NotNull Pose2d pose, @NotNull Rotation2d gyroAngle, boolean offsetGyro) {
+    public void resetPosition(@NotNull Pose2d pose, @NotNull Rotation2d gyroAngle) {
+        final Drive drive = Drive.getInstance();
         lock.writeLock().lock();
         try {
-            if (offsetGyro) {
-                gyroOffset = latestEstimatedPose.getRotation().minus(gyroAngle);
-            }
+
+            gyroOffset = pose.getRotation().minus(gyroAngle);
             swerveDriveOdometry.resetPosition(pose, gyroAngle);
-            latencyCompensatedPose = latestEstimatedPose;
+            latestEstimatedPose = pose;
 
             latestChassisSpeeds = getRotatedSpeeds(
-                    drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()),
+                    Drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates()),
                     latestEstimatedPose.getRotation());
 
             latencyCompensatedChassisSpeeds = latestChassisSpeeds;
@@ -429,12 +409,7 @@ public void resetPosition(@NotNull Pose2d pose, @NotNull Rotation2d gyroAngle, b
      */
     @Contract(pure = true)
     public @NotNull Pose2d getLatencyCompedPoseMeters() {
-        lock.readLock().lock();
-        try {
-            return latencyCompensatedPose;
-        } finally {
-            lock.readLock().unlock();
-        }
+        return getLastEstimatedPoseMeters();
     }
 
     /**
@@ -485,6 +460,7 @@ public void logData() {
             logData("Last Estimated Robot Pose X", getLastEstimatedPoseMeters().getX());
             logData("Last Estimated Robot Pose Y", getLastEstimatedPoseMeters().getY());
             logData("Last Estimated Robot Pose Angle", getLastEstimatedPoseMeters().getRotation().getRadians());
+            logData("Gyro Robot Pose Angle", getGyroAngle().getRadians());
             logData("Last Estimated Robot Velocity X", getLastChassisSpeeds().vxMetersPerSecond);
             logData("Last Estimated Robot Velocity Y", getLastChassisSpeeds().vyMetersPerSecond);
             logData("Last Estimated Robot Velocity Theta", getLastChassisSpeeds().omegaRadiansPerSecond);
diff --git a/src/main/java/frc/subsystem/Shooter.java b/src/main/java/frc/subsystem/Shooter.java
index 7b09f405..177be052 100644
--- a/src/main/java/frc/subsystem/Shooter.java
+++ b/src/main/java/frc/subsystem/Shooter.java
@@ -12,7 +12,6 @@
 import com.revrobotics.SparkMaxPIDController;
 import edu.wpi.first.networktables.EntryListenerFlags;
 import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
@@ -26,6 +25,8 @@
 import frc.utility.shooter.visionlookup.ShooterPreset;
 import org.jetbrains.annotations.NotNull;
 
+import static frc.robot.Constants.*;
+
 /**
  * Shooter class controls the shooter flywheel, feeder wheel, and variable hood Has motor control wrappers for setting velocity
  * and position for respective parts.
@@ -37,9 +38,6 @@
 
 public final class Shooter extends AbstractSubsystem {
 
-    // PID TUNING
-    final @NotNull NetworkTableInstance networkTableInstance = NetworkTableInstance.getDefault();
-
     final @NotNull NetworkTableEntry shooterP = SmartDashboard.getEntry("ShooterPIDP");
     final @NotNull NetworkTableEntry shooterI = SmartDashboard.getEntry("ShooterPIDI");
     final @NotNull NetworkTableEntry shooterD = SmartDashboard.getEntry("ShooterPIDD");
@@ -55,7 +53,6 @@ public final class Shooter extends AbstractSubsystem {
 
     // Feeder
     private final LazyTalonFX feederWheel;
-    private double forceFeederOnTime;
 
     // Hood
     private final LazyCANSparkMax hoodMotor;
@@ -84,8 +81,7 @@ public final class Shooter extends AbstractSubsystem {
     public boolean isFiring() {
         return shooterState == ShooterState.ON &&
                 ((feederWheelState == FeederWheelState.FORWARD)
-                        && ((isHoodAtTargetAngle() && isShooterAtTargetSpeed()) || feederChecksDisabled)
-                        || Timer.getFPGATimestamp() < forceFeederOnTime);
+                        && ((isHoodAtTargetAngle() && isShooterAtTargetSpeed()) || feederChecksDisabled));
     }
 
     // Declarations of Modes and States
@@ -556,31 +552,6 @@ public void forceFeederForward() {
         feederWheel.set(ControlMode.PercentOutput, Constants.FEEDER_WHEEL_SPEED);
     }
 
-    private double feederLockPosition = 0;
-
-    public void lockFeederWheel() {
-
-        // Initializes feederLockPosition if it has not been set before
-        if (feederLockPosition == 0) {
-            feederLockPosition = feederWheel.getSelectedSensorPosition();
-        }
-
-//        // Will lock feeder to position when locking starts
-//        if (Math.abs(feederWheel.getSelectedSensorVelocity() * Constants.FALCON_ENCODER_TICKS_PER_100_MS_TO_RPM) <
-//                Constants.FEEDER_WHEEL_LOCK_SPEED_RPM) {
-//            feederWheel.set(ControlMode.Position, feederLockPosition);
-//        } else {
-//            // Resets the lockPosition when not locking
-//            feederLockPosition = feederWheel.getSelectedSensorPosition();
-//            feederWheel.set(ControlMode.PercentOutput, 0);
-//        }
-
-        feederWheel.set(ControlMode.Position, feederLockPosition);
-        logData("Feeder Lock Pos ", feederLockPosition);
-        logData("Feeder lock pos time ", Timer.getFPGATimestamp());
-        //feederWheel.set(ControlMode.PercentOutput, 0);
-    }
-
     public void setHoodZero() {
         System.out.println(" Setting Zero " + hoodAbsoluteEncoder.configGetMagnetOffset() + " -> 90");
         hoodAbsoluteEncoder.configAbsoluteSensorRange(AbsoluteSensorRange.Unsigned_0_to_360);
@@ -588,8 +559,11 @@ public void setHoodZero() {
                 -(-hoodAbsoluteEncoder.getAbsolutePosition() - hoodAbsoluteEncoder.configGetMagnetOffset()) - 90);
     }
 
-    private double nextAllowedShootTime = 0;
-    private double rumbleTime = 0;
+    public double getLastShotTime() {
+        return lastShotTime;
+    }
+
+    private volatile double lastShotTime = 0;
 
     /**
      * Update Method for Shooter.
@@ -623,7 +597,6 @@ public void update() {
 
                 if (feederWheelState == FeederWheelState.REVERSE) {
                     feederWheel.set(ControlMode.PercentOutput, -Constants.FEEDER_WHEEL_SPEED);
-                    feederLockPosition = feederWheel.getSelectedSensorPosition();
                 } else {
                     if (runFeederWheelReversed) {
                         feederWheel.set(ControlMode.PercentOutput, -0.3);
@@ -637,6 +610,7 @@ public void update() {
                 break;
 
             case ON:
+
                 shooterWheelMaster.set(ControlMode.Velocity,
                         desiredShooterSpeed * Constants.SET_SHOOTER_SPEED_CONVERSION_FACTOR); // Sets shooter motor to desired shooter
 
@@ -644,26 +618,25 @@ public void update() {
                     moveHoodMotor(); // Sets Motor to travel to desired hood angle
                 }
 
-                if (nextAllowedShootTime > Timer.getFPGATimestamp() + Constants.SECOND_BALL_SHOOT_DELAY + 0.2) {
+                if (lastShotTime > Timer.getFPGATimestamp()) {
                     //Check to make sure we don't accidentally do anything dumb and prevent us from shooting. It checks if the
                     //nextAllowedShootTime is greater than the current time plus the delay time which should never happen
-                    nextAllowedShootTime = Timer.getFPGATimestamp();
+                    lastShotTime = Timer.getFPGATimestamp();
                 }
 
-                if ((feederWheelState == FeederWheelState.FORWARD)
-                        && ((isHoodAtTargetAngle() && isShooterAtTargetSpeed())
-                        && (Timer.getFPGATimestamp() > nextAllowedShootTime)
-                        || feederChecksDisabled)
+                if (
+                        ((feederWheelState == FeederWheelState.FORWARD)
+                                && ((isHoodAtTargetAngle() && isShooterAtTargetSpeed())
+                                && (Timer.getFPGATimestamp() > (VisionManager.getInstance().getDistanceToTarget() < 80 ?
+                                0.5 : SECOND_BALL_SHOOT_DELAY) + lastShotTime)
+                                || feederChecksDisabled))
                 ) {
-                    feederWheel.set(ControlMode.PercentOutput, Constants.FEEDER_WHEEL_SPEED);
-                    feederLockPosition = feederWheel.getSelectedSensorPosition();
-                    forceFeederOnTime = Timer.getFPGATimestamp() + Constants.FEEDER_CHANGE_STATE_DELAY_SEC;
-                    nextAllowedShootTime = Timer.getFPGATimestamp() + Constants.SECOND_BALL_SHOOT_DELAY;
-                    rumbleTime = Timer.getFPGATimestamp() + Constants.RUMBLE_DELAY;
+                    feederWheel.set(ControlMode.PercentOutput, FEEDER_WHEEL_SPEED);
+                    lastShotTime = Timer.getFPGATimestamp();
                     Robot.setRumble(RumbleType.kLeftRumble, 0);
                 } else {
                     // Turn OFF Feeder Wheel if feederWheel has not been on in half a second
-                    if (Timer.getFPGATimestamp() > forceFeederOnTime) {
+                    if (Timer.getFPGATimestamp() > lastShotTime + FEEDER_CHANGE_STATE_DELAY_SEC) {
                         if (runFeederWheelReversed) {
                             feederWheel.set(ControlMode.PercentOutput, -0.3);
                         } else {
@@ -671,8 +644,9 @@ public void update() {
                         }
                     }
 
-                    if (Timer.getFPGATimestamp() > rumbleTime && rumbleTime < Timer.getFPGATimestamp() + Constants.RUMBLE_TIME) {
-                        Robot.setRumble(RumbleType.kLeftRumble, 1);
+                    if (Timer.getFPGATimestamp() > lastShotTime + RUMBLE_DELAY
+                            && Timer.getFPGATimestamp() < lastShotTime + RUMBLE_DELAY + RUMBLE_TIME) {
+                        Robot.setRumble(RumbleType.kLeftRumble, 0.25);
                     } else {
                         Robot.setRumble(RumbleType.kLeftRumble, 0);
                     }
diff --git a/src/main/java/frc/subsystem/VisionManager.java b/src/main/java/frc/subsystem/VisionManager.java
index 020d3035..d4c88be9 100644
--- a/src/main/java/frc/subsystem/VisionManager.java
+++ b/src/main/java/frc/subsystem/VisionManager.java
@@ -17,26 +17,28 @@
 import frc.utility.Limelight.LedMode;
 import frc.utility.MathUtil;
 import frc.utility.Timer;
+import frc.utility.geometry.MutableTranslation2d;
 import frc.utility.shooter.visionlookup.ShooterConfig;
 import frc.utility.shooter.visionlookup.VisionLookUpTable;
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
 import org.jetbrains.annotations.Contract;
 import org.jetbrains.annotations.NotNull;
 
 import java.util.HashSet;
+import java.util.Objects;
 import java.util.Optional;
 import java.util.Set;
+import java.util.concurrent.locks.ReentrantReadWriteLock;
 
+import static frc.robot.Constants.GOAL_POSITION;
 import static frc.robot.Constants.MAX_SHOOT_SPEED;
 import static frc.utility.geometry.GeometryUtils.angleOf;
 
 public final class VisionManager extends AbstractSubsystem {
-    private static final @NotNull VisionManager instance = new VisionManager();
+    private static final ReentrantReadWriteLock VISION_MANGER_INSTANCE_LOCK = new ReentrantReadWriteLock();
+    private static VisionManager instance;
 
-    private final @NotNull RobotTracker robotTracker = RobotTracker.getInstance();
     private final @NotNull Limelight limelight = Limelight.getInstance();
-    private final @NotNull Drive drive = Drive.getInstance();
-    private final @NotNull Shooter shooter = Shooter.getInstance();
-    private final @NotNull BlinkinLED blinkinLED = BlinkinLED.getInstance();
 
     public final VisionLookUpTable visionLookUpTable = VisionLookUpTable.getInstance();
 
@@ -45,11 +47,25 @@ public void setShooterConfig(ShooterConfig shooterConfig) {
     }
 
     private VisionManager() {
-        super(Constants.VISION_MANAGER_PERIOD, 2);
+        super(Constants.VISION_MANAGER_PERIOD, 4);
     }
 
     public static @NotNull VisionManager getInstance() {
-        return instance;
+        VISION_MANGER_INSTANCE_LOCK.readLock().lock();
+        try {
+            if (instance != null) {
+                return instance;
+            }
+        } finally {
+            VISION_MANGER_INSTANCE_LOCK.readLock().unlock();
+        }
+
+        VISION_MANGER_INSTANCE_LOCK.writeLock().lock();
+        try {
+            return Objects.requireNonNullElseGet(instance, () -> instance = new VisionManager());
+        } finally {
+            VISION_MANGER_INSTANCE_LOCK.writeLock().unlock();
+        }
     }
 
     @Override
@@ -59,15 +75,19 @@ public void selfTest() {
 
     @Override
     public void logData() {
+        final @NotNull RobotTracker robotTracker = RobotTracker.getInstance();
+        final @NotNull Drive drive = Drive.getInstance();
+        final @NotNull Shooter shooter = Shooter.getInstance();
+
         logData("Distance to Target", Units.metersToInches(getDistanceToTarget()));
         logData("Rotation Target", getAngleToTarget().getDegrees());
-        logData("Allowed Turn Error", getAllowedTurnError());
+        logData("Old Distance ", limelight.getDistance() + Constants.GOAL_RADIUS_IN + 23);
+
+//        Vector2d newRelGoalPos = limelight.getCorrectGoalPos();
+//        logData("New Z", newRelGoalPos.x);
+//        logData("New X", newRelGoalPos.y);
 
         logData("Allow Shooting Robot Speed", drive.getSpeedSquared() < Constants.MAX_SHOOT_SPEED_SQUARED);
-        logData("Is Robot Allowed Shoot Aiming",
-                Math.abs((angleOf(getRelativeGoalTranslation())
-                        .minus(robotTracker.getGyroAngle())).getRadians())
-                        < getAllowedTurnError());
         logData("Is Robot Allowed Shoot Tilt",
                 Math.abs(robotTracker.getGyro().getRoll()) < 3 && Math.abs(robotTracker.getGyro().getPitch()) < 3);
 
@@ -79,22 +99,50 @@ public void logData() {
         logData("Vision Robot Velocity X", robotVelocity.getX());
         logData("Vision Robot Velocity Y", robotVelocity.getY());
 
-        Translation2d aimToPosition = getAdjustedTranslation(0.15);
+        double timeFromLastShoot = Timer.getFPGATimestamp() - shooter.getLastShotTime();
+        double shooterLookAheadTime = 0.15 - timeFromLastShoot;
+        if (shooterLookAheadTime < 0) {
+            shooterLookAheadTime = 0.15;
+        }
+
+        double turnDelay = 0.00;
+
+        Translation2d aimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay);
 
         Translation2d fieldCentricCords =
                 RobotTracker.getInstance().getLastEstimatedPoseMeters().getTranslation().minus(aimToPosition);
         logData("Calculated Target X", fieldCentricCords.getX());
         logData("Calculated Target Y", fieldCentricCords.getY());
+
+        double allowedTurnError = getAllowedTurnError(aimToPosition.getNorm());
+
+        logData("Allowed Turn Error", allowedTurnError);
+        logData("Is Robot Allowed Shoot Aiming",
+                Math.abs((angleOf(getRelativeGoalTranslation())
+                        .minus(robotTracker.getGyroAngle())).getRadians())
+                        < allowedTurnError);
+
+        logData("Acceleration", getAccel().getNorm());
     }
 
 
     public void shootAndMove(ControllerDriveInputs controllerDriveInputs, boolean useFieldRelative) {
-        Translation2d aimToPosition = getAdjustedTranslation(0.15);
+        final @NotNull Drive drive = Drive.getInstance();
+        final @NotNull Shooter shooter = Shooter.getInstance();
 
+        double timeFromLastShoot = Timer.getFPGATimestamp() - shooter.getLastShotTime();
+        double shooterLookAheadTime = 0.15 - timeFromLastShoot;
+        if (shooterLookAheadTime < 0) {
+            shooterLookAheadTime = 0.15;
+        }
+
+        double turnDelay = 0.0;
+
+        Translation2d aimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay);
         double targetAngle = angleOf(aimToPosition).getRadians();
 
         // Get the angle that will be used in the future to calculate the end velocity of the turn
-        Translation2d futureAimToPosition = getAdjustedTranslation(0.25);
+        Translation2d futureAimToPosition = getAdjustedTranslation(shooterLookAheadTime + turnDelay + 0.1);
         double futureTargetAngle = angleOf(futureAimToPosition).getRadians();
 
         drive.updateTurn(controllerDriveInputs,
@@ -102,33 +150,52 @@ public void shootAndMove(ControllerDriveInputs controllerDriveInputs, boolean us
                 useFieldRelative,
                 0);
 
-        updateShooterState(aimToPosition.getNorm());
 
-        tryToShoot(aimToPosition, (futureTargetAngle - targetAngle) * 10, false);
+        Translation2d aimChecksPosition = getAdjustedTranslation(shooterLookAheadTime);
+        updateShooterState(aimChecksPosition.getNorm());
+
+        tryToShoot(aimChecksPosition, (futureTargetAngle - targetAngle) * 10, false);
     }
 
 
     public void autoTurnRobotToTarget(ControllerDriveInputs controllerDriveInputs, boolean fieldRelative) {
-        drive.updateTurn(controllerDriveInputs, getAngleToTarget(), fieldRelative, getAllowedTurnError());
+        final @NotNull Drive drive = Drive.getInstance();
 
-        updateShooterState(getDistanceToTarget());
-        tryToShoot(getRelativeGoalTranslation(), 0, true);
+        Optional<Translation2d> visionTranslation = getVisionTranslation();
+        Translation2d relativeGoalPos;
+        if (visionTranslation.isPresent()) {
+            relativeGoalPos = visionTranslation.get().minus(GOAL_POSITION);
+        } else {
+            relativeGoalPos = getRelativeGoalTranslation();
+        }
+        drive.updateTurn(controllerDriveInputs, angleOf(relativeGoalPos), fieldRelative, getAllowedTurnError());
+        updateShooterState(relativeGoalPos.getNorm());
+        tryToShoot(relativeGoalPos, 0, true);
     }
 
     private void tryToShoot(Translation2d aimToPosition, double targetAngularSpeed, boolean doSpeedCheck) {
-        if (Math.abs((angleOf(aimToPosition).minus(robotTracker.getGyroAngle())).getRadians()) < getAllowedTurnError()
+        final @NotNull RobotTracker robotTracker = RobotTracker.getInstance();
+        final @NotNull Drive drive = Drive.getInstance();
+        final @NotNull Shooter shooter = Shooter.getInstance();
+
+        //@formatter:off
+        if (Math.abs((angleOf(aimToPosition).minus(robotTracker.getGyroAngle())).getRadians())
+                    < getAllowedTurnError(aimToPosition.getNorm())
                 && Math.abs(robotTracker.getLatencyCompedChassisSpeeds().omegaRadiansPerSecond - targetAngularSpeed)
-                < Math.toRadians(8)
-                && (drive.getSpeedSquared() < Constants.MAX_SHOOT_SPEED_SQUARED || !doSpeedCheck) &&
-                Math.abs(robotTracker.getGyro().getRoll()) < 3 && Math.abs(robotTracker.getGyro().getPitch()) < 3) {
-            shooter.setFiring(limelight.isTargetVisible() || DriverStation.isAutonomous());
+                    < Math.toRadians(8)
+                && getAccel().getNorm() < 0.75
+                && (drive.getSpeedSquared() < Constants.MAX_SHOOT_SPEED_SQUARED || !doSpeedCheck)
+                && Math.abs(robotTracker.getGyro().getRoll()) < 3 && Math.abs(robotTracker.getGyro().getPitch()) < 3) {
+            //@formatter:on
+            shooter.setFiring(true);
             if (shooter.isFiring()) {
                 if (!checksPassedLastTime && lastPrintTime + 0.5 < Timer.getFPGATimestamp()) {
                     lastPrintTime = Timer.getFPGATimestamp();
                     checksPassedLastTime = true;
                     System.out.println(
-                            "Shooting at " + (150 - DriverStation.getMatchTime()) + " "
-                                    + visionLookUpTable.getShooterPreset(Units.metersToInches(getDistanceToTarget())));
+                            "Shooting at " + (150 - DriverStation.getMatchTime()) + " Distance:  "
+                                    + Units.metersToInches(aimToPosition.getNorm()) + " "
+                                    + "Accel: " + getAccel().getNorm());
                 }
             } else {
                 lastChecksFailedTime = Timer.getFPGATimestamp();
@@ -159,6 +226,8 @@ public void updateShooterState() {
      * @param distanceToTarget the distance to the target (in meters)
      */
     public void updateShooterState(double distanceToTarget) {
+        final @NotNull Shooter shooter = Shooter.getInstance();
+
         logData("Shooter Distance to Target", Units.metersToInches(distanceToTarget));
         shooter.set(visionLookUpTable.getShooterPreset(Units.metersToInches(distanceToTarget)));
     }
@@ -168,8 +237,11 @@ public void updateShooterState(double distanceToTarget) {
      */
     @Contract(pure = true)
     public Translation2d getRobotVel() {
+        final @NotNull RobotTracker robotTracker = RobotTracker.getInstance();
+        final @NotNull Drive drive = Drive.getInstance();
+
         Rotation2d rotation2d = robotTracker.getGyroAngle();
-        ChassisSpeeds chassisSpeeds = drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates());
+        ChassisSpeeds chassisSpeeds = Drive.getSwerveDriveKinematics().toChassisSpeeds(drive.getSwerveModuleStates());
         return new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond).rotateBy(rotation2d);
     }
 
@@ -183,10 +255,15 @@ public Translation2d getRobotVel() {
 
         Rotation2d currentGyroAngle = getLatencyCompedLimelightRotation();
 
-        double distanceToTarget = limelight.getDistanceM() + Constants.GOAL_RADIUS + Units.inchesToMeters(23);
-        double angleToTarget = currentGyroAngle.getDegrees() - limelight.getHorizontalOffset();
-        return Optional.of(new Translation2d(distanceToTarget * Math.cos(Math.toRadians(angleToTarget)),
-                distanceToTarget * Math.sin(Math.toRadians(angleToTarget)))
+        Vector3D offsetVector = limelight.getCorrectTargetVector();
+        double angleOffset = Math.atan2(offsetVector.getX(), offsetVector.getZ());
+
+
+        double distanceToTarget = Units.inchesToMeters(Math.hypot(offsetVector.getX(), offsetVector.getZ()));
+
+        double angleToTarget = currentGyroAngle.getRadians() - angleOffset;
+        return Optional.of(new Translation2d(distanceToTarget * Math.cos(angleToTarget),
+                distanceToTarget * Math.sin(angleToTarget))
                 .plus(Constants.GOAL_POSITION));
     }
 
@@ -194,6 +271,7 @@ public Translation2d getRobotVel() {
      * @return current relative translation of the robot based on the robot tracker
      */
     private Translation2d getRelativeGoalTranslation() {
+        final @NotNull RobotTracker robotTracker = RobotTracker.getInstance();
         return robotTracker.getLatencyCompedPoseMeters().getTranslation()
                 .plus(robotPositionOffset)
                 .minus(Constants.GOAL_POSITION);
@@ -205,11 +283,12 @@ private Translation2d getRelativeGoalTranslation() {
      * You need to call {@link #forceVisionOn(Object)} before calling this method.
      */
     public void forceUpdatePose() {
+        final @NotNull RobotTracker robotTracker = RobotTracker.getInstance();
         Optional<Translation2d> visionTranslation = getVisionTranslation();
         visionTranslation.ifPresent(
-                mutableTranslation2d -> {
+                translation2d -> {
                     robotTracker.addVisionMeasurement(
-                            mutableTranslation2d,
+                            translation2d,
                             getLimelightTime());
                     robotPositionOffset = new Translation2d();
                 }
@@ -242,11 +321,21 @@ public double getDistanceToTarget() {
      * @return The allowed turn error in radians
      */
     private double getAllowedTurnError() {
-        return Math.tan((Constants.GOAL_RADIUS * 0.8) / getDistanceToTarget());
+        return getAllowedTurnError(getDistanceToTarget());
+    }
+
+    /**
+     * {@code Math.tan(Constants.GOAL_RADIUS / getDistanceToTarget())}
+     *
+     * @return The allowed turn error in radians
+     */
+    private double getAllowedTurnError(double distance) {
+        return Math.tan((Constants.GOAL_RADIUS * 0.5) / distance);
     }
 
     @Contract(pure = true)
     public @NotNull Rotation2d getLatencyCompedLimelightRotation() {
+        final @NotNull RobotTracker robotTracker = RobotTracker.getInstance();
         return robotTracker.getGyroRotation(getLimelightTime());
     }
 
@@ -302,7 +391,10 @@ public boolean isVisionForcedOn() {
 
     @Override
     public void update() {
-        Pose2d robotTrackerPose = robotTracker.getLatencyCompedPoseMeters();
+        final @NotNull RobotTracker robotTracker = RobotTracker.getInstance();
+        final @NotNull BlinkinLED blinkinLED = BlinkinLED.getInstance();
+
+        robotPositionOffset = new Translation2d();
         Translation2d relativeGoalPos = getRelativeGoalTranslation();
 
         double angleToTarget = Math.atan2(relativeGoalPos.getY(), relativeGoalPos.getX());
@@ -311,12 +403,14 @@ public void update() {
             blinkinLED.setStatus(limelightNotConnectedStatus);
         }
 
-        if (Math.abs(angleToTarget - robotTrackerPose.getRotation().getRadians()) < Math.toRadians(50)) {
+        if (Math.abs(new Rotation2d(angleToTarget).minus(robotTracker.getGyroAngle()).getRadians()) < Math.toRadians(50)) {
             forceVisionOn(updateLoopSource);
         } else {
             unForceVisionOn(updateLoopSource);
         }
 
+        logData("Angle To Target", angleToTarget);
+
 
         Optional<Translation2d> robotTranslationOptional = getVisionTranslation();
         if (robotTranslationOptional.isPresent()) {
@@ -328,17 +422,24 @@ public void update() {
             logData("Vision Pose Angle", visionPose.getRotation().getRadians());
             logData("Vision Pose Time", getLimelightTime());
 
-            if (MathUtil.dist2(robotTracker.getLatencyCompedPoseMeters().getTranslation().plus(robotPositionOffset),
-                    robotTranslation) < Constants.VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED
-                    && !limelight.areCornersTouchingEdge()) {
-
-                robotTracker.addVisionMeasurement(robotTranslation,
-                        getLimelightTime());
-                robotPositionOffset = new Translation2d();
+            Translation2d trackerTranslation = robotTracker.getLatencyCompedPoseMeters().getTranslation();
 
+            logData("Tracker Translation X", trackerTranslation.getX());
+            logData("Tracker Translation Y", trackerTranslation.getY());
 
-                logData("Using Vision Info", "Using Vision Info");
-                blinkinLED.setStatus(limelightUsingVisionStatus);
+            if (MathUtil.dist2(robotTracker.getLatencyCompedPoseMeters().getTranslation(),
+                    robotTranslation) < Constants.VISION_MANAGER_DISTANCE_THRESHOLD_SQUARED) {
+                if (limelight.areCornersTouchingEdge()) {
+                    logData("Using Vision Info", "Corners touching edge");
+                } else {
+                    if (!DriverStation.isAutonomous()) {
+                        robotTracker.addVisionMeasurement(robotTranslation,
+                                getLimelightTime());
+                    }
+                    robotPositionOffset = new Translation2d();
+                    logData("Using Vision Info", "Using Vision Info");
+                    blinkinLED.setStatus(limelightUsingVisionStatus);
+                }
             } else {
                 logData("Using Vision Info", "Position is too far from expected");
                 blinkinLED.setStatus(limelightTooFarFromExpectedStatus);
@@ -357,6 +458,9 @@ public void update() {
      */
     @SuppressWarnings({"unused", "BusyWait"})
     public void shootBalls(double numBalls) throws InterruptedException {
+        final @NotNull Drive drive = Drive.getInstance();
+        final @NotNull Shooter shooter = Shooter.getInstance();
+
         forceVisionOn(this);
         if (drive.driveState == DriveState.RAMSETE) {
             drive.setAutoAiming(true);
@@ -425,14 +529,24 @@ private Translation2d predictFutureTranslation(double predictAheadTime, Translat
     @NotNull Translation2d getVelocityAdjustedRelativeTranslation(
             @NotNull Translation2d relativeGoalTranslation, @NotNull Translation2d robotVelocity) {
 
-        Translation2d fakeGoalPos = relativeGoalTranslation;
+        MutableTranslation2d fakeGoalPos = new MutableTranslation2d(relativeGoalTranslation);
+
+        double relGoalX = relativeGoalTranslation.getX();
+        double relGoalY = relativeGoalTranslation.getY();
+
+        double velX = robotVelocity.getX();
+        double velY = robotVelocity.getY();
 
         for (int i = 0; i < 40; i++) {
             //System.out.println("Iteration: " + i + " Fake Goal Pos: " + fakeGoalPos);
             double tof = getTimeOfFlight(fakeGoalPos);
-            fakeGoalPos = relativeGoalTranslation.plus(robotVelocity.times(tof));
+
+            fakeGoalPos.set(
+                    relGoalX + (velX * tof),
+                    relGoalY + (velY * tof)
+            );
         }
-        return fakeGoalPos;
+        return fakeGoalPos.getTranslation2d();
     }
 
     /**
@@ -443,14 +557,14 @@ private Translation2d predictFutureTranslation(double predictAheadTime, Translat
         double distance = Units.metersToInches(translation2d.getNorm());
 
         double timeOfFlightFrames;
-        if (distance < 120) {
-            timeOfFlightFrames = 28;
+        if (distance < 113) {
+            timeOfFlightFrames = ((0.02 / 30) * (distance - 113)) + (22.0 / 30);
         } else {
-            timeOfFlightFrames = (0.09 * (distance - 120)) + 28;
+            timeOfFlightFrames = ((0.071 / 30) * (distance - 113)) + (22.0 / 30);
         }
 
         //timeOfFlightFrames = 0.000227991 * (distance * distance) - 0.0255545 * (distance) + 31.9542;
-        return timeOfFlightFrames / 30;
+        return timeOfFlightFrames;
     }
 
     /**
@@ -469,6 +583,7 @@ private Translation2d getAdjustedTranslation(double predictAheadTime) {
 
 
     private Translation2d getAccel() {
+        final @NotNull RobotTracker robotTracker = RobotTracker.getInstance();
         return robotTracker.getAcceleration();
     }
 }
diff --git a/src/main/java/frc/utility/Limelight.java b/src/main/java/frc/utility/Limelight.java
index e285f88a..082d3cbb 100644
--- a/src/main/java/frc/utility/Limelight.java
+++ b/src/main/java/frc/utility/Limelight.java
@@ -3,9 +3,14 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.EntryListenerFlags;
 import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.drive.Vector2d;
+import org.apache.commons.math3.geometry.euclidean.threed.Rotation;
+import org.apache.commons.math3.geometry.euclidean.threed.RotationConvention;
+import org.apache.commons.math3.geometry.euclidean.threed.RotationOrder;
+import org.apache.commons.math3.geometry.euclidean.threed.Vector3D;
 import org.jetbrains.annotations.NotNull;
 
 import java.util.HashMap;
@@ -26,6 +31,14 @@ public final class Limelight {
         return getInstance("limelight");
     }
 
+
+    volatile Rotation cameraRotation = new Rotation(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR, Math.toRadians(-38), 0,
+            0);
+
+    volatile double hOffset = 38;
+    volatile double depthOffset = 44;
+    volatile Vector3D centerOffset = new Vector3D(0, 0, 6.9);
+
     public static @NotNull Limelight getInstance(String name) {
         LIMELIGHT_MAP_LOCK.readLock().lock();
         try {
@@ -124,6 +137,29 @@ private Limelight(String name) {
         limelightTable = NetworkTableInstance.getDefault().getTable(name);
         limelightGuiTable = NetworkTableInstance.getDefault().getTable(name + "gui");
 
+        NetworkTableEntry angleTable = limelightGuiTable.getEntry("angle");
+        angleTable.setDouble(cameraRotation.getAngles(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR)[0]);
+        angleTable.addListener(event -> cameraRotation
+                        = new Rotation(RotationOrder.XYZ, RotationConvention.VECTOR_OPERATOR,
+                        Math.toRadians(event.getEntry().getDouble(0)), 0, 0),
+                EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+
+        NetworkTableEntry hOffsetTable = limelightGuiTable.getEntry("hOffset");
+        hOffsetTable.setDouble(hOffset);
+        hOffsetTable.addListener(event -> hOffset = event.getEntry().getDouble(0),
+                EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+
+        NetworkTableEntry depthOffsetTable = limelightGuiTable.getEntry("depthOffset");
+        depthOffsetTable.setDouble(depthOffset);
+        depthOffsetTable.addListener(event -> depthOffset = event.getEntry().getDouble(0),
+                EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+
+        NetworkTableEntry centerOffsetTable = limelightGuiTable.getEntry("centerOffset");
+        centerOffsetTable.setDouble(centerOffset.getZ());
+        centerOffsetTable.addListener(event -> centerOffset = new Vector3D(0, 0, event.getEntry().getDouble(0)),
+                EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+
+
         limelightTable.getEntry("tl").addListener(event -> lastUpdate = Timer.getFPGATimestamp(),
                 EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
 
diff --git a/src/main/java/frc/utility/shooter/visionlookup/ShooterConfig.java b/src/main/java/frc/utility/shooter/visionlookup/ShooterConfig.java
index 6f6f9d26..57a828f9 100644
--- a/src/main/java/frc/utility/shooter/visionlookup/ShooterConfig.java
+++ b/src/main/java/frc/utility/shooter/visionlookup/ShooterConfig.java
@@ -28,4 +28,14 @@ public String toString() {
                 "shooterConfigs=" + shooterConfigs +
                 '}';
     }
+
+    public void printCSV() {
+        StringBuilder sb = new StringBuilder();
+        for (ShooterPreset shooterPreset : shooterConfigs) {
+            sb.append(shooterPreset.getDistance()).append(",")
+                    .append(shooterPreset.getFlywheelSpeed()).append(",")
+                    .append(shooterPreset.getHoodEjectAngle()).append("\n");
+        }
+        System.out.println(sb);
+    }
 }
diff --git a/src/main/java/frc/utility/shooter/visionlookup/VisionLookUpTable.java b/src/main/java/frc/utility/shooter/visionlookup/VisionLookUpTable.java
index fb485ff7..a1bbd46d 100644
--- a/src/main/java/frc/utility/shooter/visionlookup/VisionLookUpTable.java
+++ b/src/main/java/frc/utility/shooter/visionlookup/VisionLookUpTable.java
@@ -110,5 +110,14 @@ public void setShooterConfig(ShooterConfig shooterConfig) {
             shooterConfigLock.unlock();
         }
     }
+
+    public void printShooterConfig() {
+        shooterConfigLock.lock();
+        try {
+            shooterConfig.printCSV();
+        } finally {
+            shooterConfigLock.unlock();
+        }
+    }
 }
 
diff --git a/src/test/java/frc/subsystem/IntakeTest.java b/src/test/java/frc/subsystem/IntakeTest.java
index 60fbe0b1..1e6f59e2 100644
--- a/src/test/java/frc/subsystem/IntakeTest.java
+++ b/src/test/java/frc/subsystem/IntakeTest.java
@@ -65,7 +65,7 @@ public void intakeDoesRunReversedWhenOpen() throws Exception {
         intakeMotorField.setAccessible(true);
         LazyCANSparkMax intakeMotor = (LazyCANSparkMax) intakeMotorField.get(intake);
 
-        assertEquals(-Constants.INTAKE_MOTOR_SPEED, intakeMotor.get(), DELTA);
+        assertEquals(-Constants.INTAKE_SPEED, intakeMotor.get(), DELTA);
     }
 
     @Test
@@ -81,6 +81,6 @@ public void intakeDoesRunWhenOpen() throws Exception {
         intakeMotorField.setAccessible(true);
         LazyCANSparkMax intakeMotor = (LazyCANSparkMax) intakeMotorField.get(intake);
 
-        assertEquals(Constants.INTAKE_MOTOR_SPEED, intakeMotor.get(), DELTA);
+        assertEquals(Constants.INTAKE_SPEED, intakeMotor.get(), DELTA);
     }
 }
\ No newline at end of file
diff --git a/src/test/java/frc/subsystem/RobotTrackerTest.java b/src/test/java/frc/subsystem/RobotTrackerTest.java
index ddc3d894..9a1005ae 100644
--- a/src/test/java/frc/subsystem/RobotTrackerTest.java
+++ b/src/test/java/frc/subsystem/RobotTrackerTest.java
@@ -49,7 +49,7 @@ void robotTrackerTest() throws NoSuchFieldException, IllegalAccessException, NoS
             double period = 0.05;
 
             Timer.setTime(0);
-            robotTracker.resetPosition(new Pose2d(x, y, new Rotation2d(theta)), new Rotation2d(0), true);
+            robotTracker.resetPosition(new Pose2d(x, y, new Rotation2d(theta)), new Rotation2d(0));
 
             double largeRandomX = random.nextDouble() * 10 - 5;
             double largeRandomY = random.nextDouble() * 10 - 5;
diff --git a/src/test/java/frc/subsystem/VisionManagerTest.java b/src/test/java/frc/subsystem/VisionManagerTest.java
index 9029975c..d005f8e7 100644
--- a/src/test/java/frc/subsystem/VisionManagerTest.java
+++ b/src/test/java/frc/subsystem/VisionManagerTest.java
@@ -13,7 +13,7 @@ class VisionManagerTest {
     void getVelocityAdjustedRelativeTranslation() {
 
         Random random = new Random(719479);
-        for (int i = 0; i < 1000; i++) {
+        for (int i = 0; i < 10000000; i++) {
             Translation2d goalPos = new Translation2d((random.nextDouble() * 20) - 10, (random.nextDouble() * 20) - 10);
             Translation2d robotVelocity = new Translation2d((random.nextDouble() * 10) - 5, (random.nextDouble() * 10) - 5);
             Translation2d fakeGoal = VisionManager.getInstance().getVelocityAdjustedRelativeTranslation(goalPos, robotVelocity);