Add game piece positions and score positions
Most of the positions are tuned on the field. Add state tracking to
joystick_reader so we can start to manage the growing quantity of
states.
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: Ic19c6baa1ec46ace954c123e08d05c41458ff6c6
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index 011095b..13fb990 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -379,7 +379,8 @@
# Draw current spline in black
color = [0, 0, 0]
else:
- color = [0.2, random.random(), random.random()]
+ color = [0, random.random(), 1]
+ random.shuffle(color)
set_color(cr, Color(color[0], color[1], color[2]))
self.segments[i].DrawTo(cr, self.theta_version)
with px(cr):
@@ -559,4 +560,5 @@
arm_ui = ArmUi()
arm_ui.segments = graph_paths.segments
+print('Starting with segment: ', arm_ui.segments[arm_ui.index].name)
basic_window.RunApp()
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 87ced83..c017656 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -6,37 +6,169 @@
joint_center[1] + l2 - l1,
0.0,
circular_index=1)
-neutral_to_pickup_1 = np.array([2.396694, 0.508020])
-neutral_to_pickup_2 = np.array([2.874513, 0.933160])
-pickup_pos = to_theta_with_circular_index_and_roll(0.6,
- 0.4,
- np.pi / 2.0,
- circular_index=0)
-neutral_to_pickup_control_alpha_rolls = [(0.33, 0.0), (.95, np.pi / 2.0)]
+# NeutralToGroundPickupBackConeUp
+neutral_to_cone_up_1 = np.array([3.170156, -0.561227])
+neutral_to_cone_up_2 = np.array([2.972776, -1.026820])
+ground_pickup_back_cone_up = to_theta_with_circular_index_and_roll(
+ -0.913162844198605, 0.35, np.pi / 2.0, circular_index=1)
+
+# NeutralToGroundPickupBackConeDown
+neutral_to_ground_pickup_back_cone_down_1 = np.array([3.170156, -0.561227])
+neutral_to_ground_pickup_back_cone_down_2 = np.array([2.972776, -1.026820])
+ground_pickup_back_cone_down = to_theta_with_circular_index_and_roll(
+ -0.95, 0.24, np.pi / 2.0, circular_index=1)
+
+# NeutralToBackMidConeUpScore
+neutral_to_score_back_mid_cone_up_1 = np.array([0.994244, -1.417442])
+neutral_to_score_back_mid_cone_up_2 = np.array([1.711325, -0.679748])
+score_back_mid_cone_up_pos = to_theta_with_circular_index_and_roll(
+ -1.255555, 1.10, np.pi / 2.0, circular_index=0)
+
+# NeutralToMidConeDownScore
+neutral_to_score_mid_cone_down_1 = np.array([3.394572, -0.239378])
+neutral_to_score_mid_cone_down_2 = np.array([3.654854, -0.626835])
+score_mid_cone_down_pos = to_theta_with_circular_index_and_roll(
+ -1.23, 0.74, np.pi / 2.0, circular_index=1)
+
+# NeutralToMidConeDownScore
+neutral_to_hp_pickup_back_cone_up_1 = np.array([2.0, -0.239378])
+neutral_to_hp_pickup_back_cone_up_2 = np.array([1.6, -0.626835])
+neutral_to_hp_pickup_back_cone_up_alpha_rolls = [
+ (0.7, 0.0),
+ (.9, np.pi / 2.0),
+]
+hp_pickup_back_cone_up = to_theta_with_circular_index_and_roll(
+ -0.94, 1.31, np.pi / 2.0, circular_index=0)
+
+# NeutralToFrontHighConeUpScore
+neutral_to_score_front_high_cone_up_1 = np.array([2.594244, 0.417442])
+neutral_to_score_front_high_cone_up_2 = np.array([1.51325, 0.679748])
+score_front_high_cone_up_pos = to_theta_with_circular_index_and_roll(
+ 0.87, 1.26, -np.pi / 2.0, circular_index=0)
+
+# NeutralToFrontMidConeUpScore
+neutral_to_score_front_mid_cone_up_1 = np.array([3.0, 0.317442])
+neutral_to_score_front_mid_cone_up_2 = np.array([2.9, 0.479748])
+score_front_mid_cone_up_pos = to_theta_with_circular_index_and_roll(
+ 0.34, 0.93, -np.pi / 2.0, circular_index=0)
+
+neutral_to_cone_down_1 = np.array([2.396694, 0.508020])
+neutral_to_cone_down_2 = np.array([2.874513, 0.933160])
+cone_down_pos = to_theta_with_circular_index_and_roll(0.7,
+ 0.11,
+ np.pi / 2.0,
+ circular_index=0)
+
+neutral_to_cube_1 = np.array([2.396694, 0.508020])
+neutral_to_cube_2 = np.array([2.874513, 0.933160])
+
+cube_pos = to_theta_with_circular_index_and_roll(0.7,
+ 0.24,
+ np.pi / 2.0,
+ circular_index=0)
+
+neutral_to_pickup_control_alpha_rolls = [
+ (0.30, 0.0),
+ (.95, np.pi / 2.0),
+]
neutral_to_score_1 = np.array([0.994244, -1.417442])
neutral_to_score_2 = np.array([1.711325, -0.679748])
-score_pos = to_theta_with_circular_index_and_roll(-1.0,
- 1.2,
- np.pi / 2.0,
- circular_index=0)
-neutral_to_score_control_alpha_rolls = [(0.40, 0.0), (.95, np.pi / 2.0)]
+score_low_pos = to_theta_with_circular_index_and_roll(-(0.41 / 2 + 0.49),
+ 0 + 0.05,
+ np.pi / 2.0,
+ circular_index=1)
-# TODO(Max): Add real paths for arm.
-points = [(neutral, "NeutralPos"), (pickup_pos, "PickupPos"),
- (score_pos, "ScorePos")]
+neutral_to_score_low_2 = np.array([3.37926599, -0.73664663])
+
+score_mid_cube_pos = to_theta_with_circular_index_and_roll(-(0.58 + 0.49),
+ 0.6 + 0.05,
+ np.pi / 2.0,
+ circular_index=0)
+
+score_high_cone_pos = to_theta_with_circular_index_and_roll(-1.01,
+ 1.17 + 0.05,
+ np.pi / 2.0,
+ circular_index=0)
+
+score_high_cube_pos = to_theta_with_circular_index_and_roll(-1.01,
+ 0.90 + 0.05,
+ np.pi / 2.0,
+ circular_index=0)
+
+neutral_to_back_score_control_alpha_rolls = [(0.40, 0.0), (.95, np.pi / 2.0)]
+neutral_to_front_score_control_alpha_rolls = [(0.40, 0.0), (.95, -np.pi / 2.0)]
+
+points = [(neutral, "NeutralPos"),
+ (ground_pickup_back_cone_up, "GroundPickupBackConeUp"),
+ (ground_pickup_back_cone_down, "GroundPickupBackConeDown"),
+ (hp_pickup_back_cone_up, "HPPickupBackConeUp"),
+ (cone_down_pos, "ConeDownPos"), (score_low_pos, "ScoreLowPos"),
+ (score_back_mid_cone_up_pos, "ScoreBackMidConeUpPos"),
+ (score_front_high_cone_up_pos, "ScoreFrontHighConeUpPos"),
+ (score_front_mid_cone_up_pos, "ScoreFrontMidConeUpPos"),
+ (score_mid_cone_down_pos, "ScoreBackMidConeDownPos"),
+ (score_mid_cube_pos, "ScoreMidCubePos"),
+ (score_high_cone_pos, "ScoreHighConePos"),
+ (score_high_cube_pos, "ScoreHighCubePos"), (cube_pos, "CubePos")]
front_points = []
back_points = []
unnamed_segments = []
named_segments = [
- ThetaSplineSegment("NeutralToPickup", neutral, neutral_to_pickup_1,
- neutral_to_pickup_2, pickup_pos,
+ ThetaSplineSegment("NeutralToGroundPickupBackConeUp", neutral,
+ neutral_to_cone_up_1, neutral_to_cone_up_2,
+ ground_pickup_back_cone_up,
neutral_to_pickup_control_alpha_rolls),
- ThetaSplineSegment("NeutralToScore", neutral, neutral_to_score_1,
- neutral_to_score_2, score_pos,
- neutral_to_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToGroundPickupBackConeDown", neutral,
+ neutral_to_ground_pickup_back_cone_down_1,
+ neutral_to_ground_pickup_back_cone_down_2,
+ ground_pickup_back_cone_down,
+ neutral_to_pickup_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToHPPickupBackConeUp", neutral,
+ neutral_to_hp_pickup_back_cone_up_1,
+ neutral_to_hp_pickup_back_cone_up_2,
+ hp_pickup_back_cone_up,
+ neutral_to_hp_pickup_back_cone_up_alpha_rolls),
+ ThetaSplineSegment("NeutralToFrontHighConeUpScore", neutral,
+ neutral_to_score_front_high_cone_up_1,
+ neutral_to_score_front_high_cone_up_2,
+ score_front_high_cone_up_pos,
+ neutral_to_front_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToFrontMidConeUpScore", neutral,
+ neutral_to_score_front_mid_cone_up_1,
+ neutral_to_score_front_mid_cone_up_2,
+ score_front_mid_cone_up_pos,
+ neutral_to_front_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToConeDown", neutral, neutral_to_cone_down_1,
+ neutral_to_cone_down_2, cone_down_pos,
+ neutral_to_pickup_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToCube", neutral, neutral_to_cube_1,
+ neutral_to_cube_2, cube_pos,
+ neutral_to_pickup_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToLowScore", neutral, neutral_to_score_1,
+ neutral_to_score_low_2, score_low_pos,
+ neutral_to_back_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToBackMidConeUpScore", neutral,
+ neutral_to_score_back_mid_cone_up_1,
+ neutral_to_score_back_mid_cone_up_2,
+ score_back_mid_cone_up_pos,
+ neutral_to_back_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToMidConeDownScore", neutral,
+ neutral_to_score_mid_cone_down_1,
+ neutral_to_score_mid_cone_down_2,
+ score_mid_cone_down_pos,
+ neutral_to_back_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToMidCubeScore", neutral, neutral_to_score_1,
+ neutral_to_score_2, score_mid_cube_pos,
+ neutral_to_back_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToHighConeScore", neutral, neutral_to_score_1,
+ neutral_to_score_2, score_high_cone_pos,
+ neutral_to_back_score_control_alpha_rolls),
+ ThetaSplineSegment("NeutralToHighCubeScore", neutral, neutral_to_score_1,
+ neutral_to_score_2, score_high_cube_pos,
+ neutral_to_back_score_control_alpha_rolls),
]
segments = named_segments + unnamed_segments