commit 3d0beaf4bb8765d6a2cdbe840da2683b78adfb74
author Maxwell Henderson <mxwhenderson@gmail.com> Thu Mar 23 11:32:44 2023 -0700
committer Maxwell Henderson <mxwhenderson@gmail.com> Thu Mar 23 11:43:27 2023 -0700
tree 4a6c1b6e10911f9e64504a595684c57ba3960504
parent 97b2c0565ed8e6b103cc4578905f70ebb8478ad1

Add 3 game piece auto

Change-Id: I3ee12fe6f253705e825774620d16b00c266db30d
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2023/control_loops/python/graph_paths.py

diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 24b9805..557d609 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -382,17 +382,19 @@
         control_alpha_rolls=[(0.40, 0.0), (.95, -np.pi / 2.0)],
     ))
 
+points['Starting'] = np.array((np.pi, -0.125053863467887, 0.0))
+
 points['ScoreFrontMidConeUpAuto'] = to_theta_with_circular_index_and_roll(
     0.58, 0.97, -np.pi / 2.0, circular_index=0)
 
 named_segments.append(
     ThetaSplineSegment(
-        name="NeutralToScoreFrontMidConeUpAuto",
-        start=points['Neutral'],
+        name="StartingToScoreFrontMidConeUpAuto",
+        start=points['Starting'],
         control1=np.array([2.99620794024176, 0.23620211875551145]),
         control2=np.array([2.728197531599509, 0.5677148040671784]),
         end=points['ScoreFrontMidConeUpAuto'],
-        control_alpha_rolls=[(0.20, 0.0), (.90, -np.pi / 2.0)],
+        control_alpha_rolls=[(0.20, 0.0), (.85, -np.pi / 2.0)],
         vmax=10.0,
         alpha_unitizer=np.matrix(
             f"{1.0 / 20.0} 0 0; 0 {1.0 / 25.0} 0; 0 0 {1.0 / 100.0}"),
@@ -444,17 +446,6 @@
         control_alpha_rolls=[],
     ))
 
-# Auto express spline...
-named_segments.append(
-    ThetaSplineSegment(
-        name="GroundPickupBackCubeToScoreFrontMidCube",
-        start=points['ScoreFrontMidCube'],
-        control1=np.array([3.2345111429709847, 0.45338639767112277]),
-        control2=np.array([3.098240119468829, -0.46161157069783254]),
-        end=points['GroundPickupBackCube'],
-        control_alpha_rolls=[(0.40, 0.0), (0.60, 0.0)],
-    ))
-
 points['ScoreFrontHighCube'] = to_theta_with_circular_index_and_roll(
     0.901437, 1.16, np.pi / 2.0, circular_index=0)
 
@@ -656,6 +647,27 @@
         control_alpha_rolls=[],
     ))
 
+# Auto express spline...
+named_segments.append(
+    ThetaSplineSegment(
+        name="GroundPickupBackCubeToScoreFrontMidCube",
+        start=points['ScoreFrontMidCube'],
+        control1=np.array([3.2345111429709847, 0.45338639767112277]),
+        control2=np.array([3.098240119468829, -0.46161157069783254]),
+        end=points['GroundPickupBackCube'],
+        control_alpha_rolls=[(0.40, 0.0), (0.60, 0.0)],
+    ))
+
+named_segments.append(
+    ThetaSplineSegment(
+        name="GroundPickupBackCubeToScoreFrontHighCube",
+        start=points['ScoreFrontHighCube'],
+        control1=np.array([2.7074513232200186, 0.20154350392334375]),
+        control2=np.array([3.01714846217257, -0.6310437434614364]),
+        end=points['GroundPickupBackCube'],
+        control_alpha_rolls=[(0.40, 0.0), (0.60, 0.0)],
+    ))
+
 front_points = []
 back_points = []
 unnamed_segments = []