Add all the rest of the upright and fallen over cone positions
This doesn't do the point towards us positions, but does the easy ones.
It also adds some direct connections between score positions.
Change-Id: I74c5b0e7c83875852ae3f615ca0f0b1d6499187d
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/constants.cc b/y2023/constants.cc
index 7e30898..6b04963 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -82,28 +82,29 @@
break;
case kCompTeamNumber:
- arm_proximal->zeroing.measured_absolute_position = 0.149079699400425;
+ arm_proximal->zeroing.measured_absolute_position = 0.132182297391884;
arm_proximal->potentiometer_offset =
0.931355973012855 + 8.6743197253382 - 0.101200335326309 -
0.0820901660993467 - 0.0703733798337964;
- arm_distal->zeroing.measured_absolute_position = 0.58478872393942;
+ arm_distal->zeroing.measured_absolute_position = 0.597004611319487;
arm_distal->potentiometer_offset =
0.436664933370656 + 0.49457213779426 + 6.78213223139724 -
0.0220711555235029 - 0.0162945074111813 + 0.00630344935527365 -
- 0.0164398318919943 - 0.145833494945215 + 0.234878799868491;
+ 0.0164398318919943 - 0.145833494945215 + 0.234878799868491 +
+ 0.125924230298394;
- roll_joint->zeroing.measured_absolute_position = 1.7229611246564;
+ roll_joint->zeroing.measured_absolute_position = 0.593975883699743;
roll_joint->potentiometer_offset =
-(3.87038557084874 - 0.0241774522172967 + 0.0711345168020632 -
0.866186131631967 - 0.0256788357596952 + 0.18101759154572017 -
0.0208958996127179 - 0.186395903925026 + 0.45801689548395 -
0.5935210745062 + 0.166256655718334 - 0.12591438680483 +
0.11972765117321 - 0.318724743041507) +
- 0.0201047336425017;
+ 0.0201047336425017 - 1.0173426655158 - 0.186085272847293 - 0.0317706563397807;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 0.85247856535298;
+ 1.54674994866259;
break;
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index cde885c..6c74a74 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -8,7 +8,7 @@
points['Neutral'] = np.array((np.pi, 0.0, 0.0))
points['GroundPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
- -1.07774334, 0.36308701, np.pi / 2.0, circular_index=1)
+ -1.07774334, 0.40, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -20,21 +20,74 @@
control_alpha_rolls=[(0.30, 0.0), (.95, np.pi / 2.0)],
))
-points['GroundPickupBackConeDown'] = to_theta_with_circular_index_and_roll(
- -1.11487594, 0.23140145, np.pi / 2.0, circular_index=1)
+points['GroundPickupBackConeDownBase'] = to_theta_with_circular_index_and_roll(
+ -1.11487594, 0.25, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
- name="NeutralToGroundPickupBackConeDown",
+ name="NeutralToGroundPickupBackConeDownBase",
start=points['Neutral'],
control1=np.array([3.170156, -0.561227]),
control2=np.array([2.972776, -1.026820]),
- end=points['GroundPickupBackConeDown'],
+ end=points['GroundPickupBackConeDownBase'],
control_alpha_rolls=[(0.30, 0.0), (.95, np.pi / 2.0)],
))
+points[
+ 'GroundPickupFrontConeDownBase'] = to_theta_with_circular_index_and_roll(
+ 0.263207, 0.24, -np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToGroundPickupFrontConeDownBase",
+ start=points['Neutral'],
+ control1=np.array([3.495221564200401, 0.4737763579250964]),
+ control2=np.array([4.110392601248856, 1.0424853539638115]),
+ end=points['GroundPickupFrontConeDownBase'],
+ control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
+ ))
+
+points['ScoreFrontLowConeDownBase'] = to_theta_with_circular_index_and_roll(
+ 0.328533, 0.40, -np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontLowConeDownBase",
+ start=points['Neutral'],
+ control1=np.array([3.153481004695907, 0.4827717171390571]),
+ control2=np.array([4.107487625131798, 0.9935705415901082]),
+ end=points['ScoreFrontLowConeDownBase'],
+ control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
+ ))
+
+points['ScoreFrontMidConeDownBase'] = to_theta_with_circular_index_and_roll(
+ 0.697179, 0.88, -np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontMidConeDownBase",
+ start=points['Neutral'],
+ control1=np.array([3.2296966803523395, 0.4274365560093907]),
+ control2=np.array([3.111677631381042, 0.6783534686461494]),
+ end=points['ScoreFrontMidConeDownBase'],
+ control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
+ ))
+
+points['ScoreFrontHighConeDownBase'] = to_theta_with_circular_index_and_roll(
+ 1.04686, 1.13243, -np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontHighConeDownBase",
+ start=points['Neutral'],
+ control1=np.array([2.7653359284612185, 0.3091554519868296]),
+ control2=np.array([2.6035409027556344, 0.5009078441624968]),
+ end=points['ScoreFrontHighConeDownBase'],
+ control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
+ ))
+
points['GroundPickupBackCube'] = to_theta_with_circular_index_and_roll(
- -1.102, 0.25, -np.pi / 2.0, circular_index=1)
+ -1.102, 0.30, -np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
@@ -59,34 +112,80 @@
control_alpha_rolls=[(0.4, 0.0), (.9, np.pi / 2.0)],
))
-points['ScoreBackMidConeUpPos'] = to_theta_with_circular_index_and_roll(
- -1.41871454, 1.07476162, np.pi / 2.0, circular_index=0)
+points['ScoreBackMidConeUp'] = to_theta_with_circular_index_and_roll(
+ -1.33013, 1.08354, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
name="NeutralToBackMidConeUpScore",
start=points['Neutral'],
- control1=np.array([0.994244, -1.417442]),
- control2=np.array([1.711325, -0.679748]),
- end=points['ScoreBackMidConeUpPos'],
+ control1=np.array([3.6130298244820453, -0.2781204657180023]),
+ control2=np.array([3.804763224169111, -0.5179424890517237]),
+ end=points['ScoreBackMidConeUp'],
control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
))
-points['ScoreBackMidConeDownPos'] = to_theta_with_circular_index_and_roll(
+points['ScoreBackLowConeUp'] = to_theta_with_circular_index_and_roll(
+ -1.00472, 0.672615, np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToBackLowConeUpScore",
+ start=points['Neutral'],
+ control1=np.array([3.260123029490386, -0.5296803702636037]),
+ control2=np.array([3.1249665389044283, -0.7810758529482493]),
+ end=points['ScoreBackLowConeUp'],
+ control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="GroundPickupBackConeUpToBackLowConeUpScore",
+ start=points['GroundPickupBackConeUp'],
+ control1=np.array([2.943017165830755, -1.3740647485244808]),
+ control2=np.array([2.941104610508278, -1.2434759967435083]),
+ end=points['ScoreBackLowConeUp'],
+ control_alpha_rolls=[],
+ ))
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="ScoreBackLowConeUpToScoreBackMidConeUp",
+ start=points['ScoreBackLowConeUp'],
+ control1=np.array([3.2930271753937728, -0.9256552441650734]),
+ control2=np.array([3.6425461598470568, -0.8085366888146934]),
+ end=points['ScoreBackMidConeUp'],
+ control_alpha_rolls=[],
+ ))
+
+points['ScoreBackMidConeDownBase'] = to_theta_with_circular_index_and_roll(
-1.37792406, 0.81332449, np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
- name="NeutralToMidConeDownScore",
+ name="NeutralToMidConeDownBaseScore",
start=points['Neutral'],
control1=np.array([3.394572, -0.239378]),
control2=np.array([3.654854, -0.626835]),
- end=points['ScoreBackMidConeDownPos'],
+ end=points['ScoreBackMidConeDownBase'],
+ control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
+ ))
+
+points['ScoreBackLowConeDownBase'] = to_theta_with_circular_index_and_roll(
+ -1.06372, 0.442764, np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToLowConeDownBaseScore",
+ start=points['Neutral'],
+ control1=np.array([2.8613439132427896, -0.5868069120126034]),
+ control2=np.array([2.9041434685529923, -1.240030040719494]),
+ end=points['ScoreBackLowConeDownBase'],
control_alpha_rolls=[(0.40, 0.0), (.95, np.pi / 2.0)],
))
points['HPPickupBackConeUp'] = to_theta_with_circular_index_and_roll(
- -1.1050539, 1.31390128, np.pi / 2.0, circular_index=0)
+ -1.1050539, 1.34, np.pi / 2.0, circular_index=0)
named_segments.append(
ThetaSplineSegment(
@@ -98,7 +197,22 @@
control_alpha_rolls=[(0.7, 0.0), (.9, np.pi / 2.0)],
))
-points['ScoreFrontHighConeUpPos'] = to_theta_with_circular_index_and_roll(
+points['HPPickupFrontConeUp'] = np.array(
+ (5.16514378449353, 1.26, -np.pi / 2.0))
+# to_theta_with_circular_index_and_roll(
+# 0.265749, 1.28332, -np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToHPPickupFrontConeUp",
+ start=points['Neutral'],
+ control1=np.array([4.068204933788692, -0.05440997896697275]),
+ control2=np.array([4.861911360838861, -0.03790410600482508]),
+ end=points['HPPickupFrontConeUp'],
+ control_alpha_rolls=[(0.7, 0.0), (.9, -np.pi / 2.0)],
+ ))
+
+points['ScoreFrontHighConeUp'] = to_theta_with_circular_index_and_roll(
0.98810344, 1.37536719, -np.pi / 2.0, circular_index=0)
named_segments.append(
@@ -107,11 +221,11 @@
start=points['Neutral'],
control1=np.array([2.594244, 0.417442]),
control2=np.array([1.51325, 0.679748]),
- end=points['ScoreFrontHighConeUpPos'],
+ end=points['ScoreFrontHighConeUp'],
control_alpha_rolls=[(0.40, 0.0), (.95, -np.pi / 2.0)],
))
-points['ScoreFrontMidConeUpPos'] = to_theta_with_circular_index_and_roll(
+points['ScoreFrontMidConeUp'] = to_theta_with_circular_index_and_roll(
0.43740453, 1.06330555, -np.pi / 2.0, circular_index=0)
named_segments.append(
@@ -120,7 +234,7 @@
start=points['Neutral'],
control1=np.array([3.0, 0.317442]),
control2=np.array([2.9, 0.479748]),
- end=points['ScoreFrontMidConeUpPos'],
+ end=points['ScoreFrontMidConeUp'],
control_alpha_rolls=[(0.40, 0.0), (.95, -np.pi / 2.0)],
))
@@ -137,19 +251,6 @@
control_alpha_rolls=[(0.4, 0.0), (.9, np.pi / 2.0)],
))
-points['ScoreBackLowCube'] = to_theta_with_circular_index_and_roll(
- -1.102, 0.30, -np.pi / 2.0, circular_index=1)
-
-named_segments.append(
- ThetaSplineSegment(
- name="NeutralToScoreLowBackCube",
- start=points['Neutral'],
- control1=np.array([3.153228, -0.497009]),
- control2=np.array([2.972776, -1.026820]),
- end=points['ScoreBackLowCube'],
- control_alpha_rolls=[(0.7, 0.0), (.9, -np.pi / 2.0)],
- ))
-
points['ScoreFrontMidCube'] = to_theta_with_circular_index_and_roll(
0.517846, 0.87, np.pi / 2.0, circular_index=0)
@@ -163,6 +264,16 @@
control_alpha_rolls=[(0.4, np.pi * 0.0), (0.95, np.pi * 0.5)],
))
+named_segments.append(
+ ThetaSplineSegment(
+ name="ScoreFrontLowCubeToScoreFrontMidCube",
+ start=points["ScoreFrontLowCube"],
+ control1=np.array([3.8237323383577078, 1.2979562720646056]),
+ control2=np.array([3.63484177908944, 1.008850428344438]),
+ end=points["ScoreFrontMidCube"],
+ control_alpha_rolls=[],
+ ))
+
points['ScoreFrontHighCube'] = to_theta_with_circular_index_and_roll(
0.901437, 1.16, np.pi / 2.0, circular_index=0)
@@ -176,6 +287,29 @@
control_alpha_rolls=[(0.4, np.pi * 0.0), (0.95, np.pi * 0.5)],
))
+named_segments.append(
+ ThetaSplineSegment(
+ name="ScoreFrontMidCubeToScoreFrontHighCube",
+ start=points["ScoreFrontMidCube"],
+ control1=np.array([2.9229652375897004, 0.7771801809056819]),
+ control2=np.array([2.634276444896239, 0.5696525540129302]),
+ end=points["ScoreFrontHighCube"],
+ control_alpha_rolls=[],
+ ))
+
+points['ScoreBackLowCube'] = to_theta_with_circular_index_and_roll(
+ -1.102, 0.30, -np.pi / 2.0, circular_index=1)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreLowBackCube",
+ start=points['Neutral'],
+ control1=np.array([3.153228, -0.497009]),
+ control2=np.array([2.972776, -1.026820]),
+ end=points['ScoreBackLowCube'],
+ control_alpha_rolls=[(0.7, 0.0), (.9, -np.pi / 2.0)],
+ ))
+
points['ScoreBackMidCube'] = to_theta_with_circular_index_and_roll(
-1.27896, 0.84, -np.pi / 2.0, circular_index=1)
@@ -189,6 +323,16 @@
control_alpha_rolls=[(0.3, -np.pi * 0.0), (0.95, -np.pi * 0.5)],
))
+named_segments.append(
+ ThetaSplineSegment(
+ name="ScoreBackLowCubeToScoreBackMidCube",
+ start=points["ScoreBackLowCube"],
+ control1=np.array([3.1075630474968694, -1.1675095818664531]),
+ control2=np.array([3.3377520447373232, -1.1054408842366303]),
+ end=points["ScoreBackMidCube"],
+ control_alpha_rolls=[],
+ ))
+
# TODO(austin): This doesn't produce the next line...
#points['ScoreBackHighCube'] = to_theta_with_circular_index_and_roll(
# -1.60932, 1.16839, np.pi / 2.0, circular_index=0)
@@ -205,18 +349,70 @@
control_alpha_rolls=[(0.3, -np.pi * 0.0), (0.95, -np.pi * 0.5)],
))
-points['ConeDownPos'] = to_theta_with_circular_index_and_roll(0.7,
- 0.11,
- np.pi / 2.0,
- circular_index=0)
named_segments.append(
ThetaSplineSegment(
- name="NeutralToConeDown",
+ name="ScoreBackMidCubeToScoreBackHighCube",
+ start=points["ScoreBackMidCube"],
+ control1=np.array([4.03651864313893, -0.919229198708873]),
+ control2=np.array([4.377346803653962, -1.0167608157302999]),
+ end=points["ScoreBackHighCube"],
+ control_alpha_rolls=[],
+ ))
+
+points['GroundPickupFrontConeUp'] = to_theta_with_circular_index_and_roll(
+ 0.313099, 0.380, -np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToGroundPickupFrontConeUp",
start=points['Neutral'],
- control1=np.array([2.396694, 0.508020]),
- control2=np.array([2.874513, 0.933160]),
- end=points['ConeDownPos'],
- control_alpha_rolls=[(0.30, 0.0), (.95, np.pi / 2.0)],
+ control1=np.array([3.153481004695907, 0.4827717171390571]),
+ control2=np.array([4.107487625131798, 0.9935705415901082]),
+ end=points['GroundPickupFrontConeUp'],
+ control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
+ ))
+
+points['ScoreFrontLowConeUp'] = to_theta_with_circular_index_and_roll(
+ 0.349687, 0.468804, -np.pi / 2.0, circular_index=0)
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="NeutralToScoreFrontLowConeUp",
+ start=points['Neutral'],
+ control1=np.array([3.153481004695907, 0.4827717171390571]),
+ control2=np.array([4.107487625131798, 0.9935705415901082]),
+ end=points['ScoreFrontLowConeUp'],
+ control_alpha_rolls=[(0.30, 0.0), (.95, -np.pi / 2.0)],
+ ))
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="GroundPickupFrontConeUpToScoreFrontLowConeUp",
+ start=points['GroundPickupFrontConeUp'],
+ control1=np.array([4.14454438793702, 1.680256664914554]),
+ control2=np.array([4.159014136030164, 1.6617266432775355]),
+ end=points['ScoreFrontLowConeUp'],
+ control_alpha_rolls=[],
+ ))
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="ScoreFrontLowConeUpToScoreFrontMidConeUp",
+ start=points['ScoreFrontLowConeUp'],
+ control1=np.array([4.144103145250675, 1.3519566301042056]),
+ control2=np.array([3.5357641970552223, 0.8105698293886593]),
+ end=points['ScoreFrontMidConeUp'],
+ control_alpha_rolls=[],
+ ))
+
+named_segments.append(
+ ThetaSplineSegment(
+ name="ScoreFrontMidConeUpToScoreFrontHighConeUp",
+ start=points['ScoreFrontMidConeUp'],
+ control1=np.array([2.417981958011055, 0.48234108399079134]),
+ control2=np.array([2.1651435746478045, 0.4937628492739232]),
+ end=points['ScoreFrontHighConeUp'],
+ control_alpha_rolls=[],
))
front_points = []
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 220f3d8..3252030 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -765,7 +765,7 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpPosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -791,7 +791,7 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpPosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -808,7 +808,7 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpPosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -819,7 +819,7 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::ConeDownPosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreFrontLowConeUpIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index a7fed8f..2cf9436 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -51,12 +51,15 @@
const ButtonLocation kMidConeScoreLeft(4, 15);
const ButtonLocation kMidConeScoreRight(3, 2);
+const ButtonLocation kLowConeScoreLeft(4, 16);
+const ButtonLocation kLowConeScoreRight(3, 3);
+
const ButtonLocation kHighCube(4, 1);
const ButtonLocation kMidCube(4, 2);
const ButtonLocation kLowCube(4, 3);
const ButtonLocation kGroundPickupConeUp(4, 7);
-const ButtonLocation kGroundPickupConeDown(4, 8);
+const ButtonLocation kGroundPickupConeDownBase(4, 8);
const ButtonLocation kGroundPickupCube(4, 10);
const ButtonLocation kHPConePickup(4, 6);
@@ -99,40 +102,118 @@
.side = Side::BACK,
},
{
- .index = arm::GroundPickupBackConeDownIndex(),
+ .index = arm::GroundPickupFrontConeUpIndex(),
+ .wrist_goal = 0.2,
+ .game_piece = GamePiece::CONE_UP,
+ .buttons = {kGroundPickupConeUp},
+ .side = Side::FRONT,
+ },
+ {
+ .index = arm::GroundPickupBackConeDownBaseIndex(),
.wrist_goal = 0.0,
.game_piece = GamePiece::CONE_DOWN,
- .buttons = {kGroundPickupConeDown},
+ .buttons = {kGroundPickupConeDownBase},
.side = Side::BACK,
},
{
- .index = arm::ScoreBackMidConeUpPosIndex(),
- .wrist_goal = 0.55,
+ .index = arm::GroundPickupFrontConeDownBaseIndex(),
+ .wrist_goal = 0.2,
+ .game_piece = GamePiece::CONE_DOWN,
+ .buttons = {kGroundPickupConeDownBase},
+ .side = Side::FRONT,
+ },
+ {
+ .index = arm::ScoreBackMidConeUpIndex(),
+ .wrist_goal = 0.0,
.game_piece = GamePiece::CONE_UP,
- .buttons = {kMidConeScoreRight},
+ .buttons = {kMidConeScoreRight, kMidConeScoreLeft},
.side = Side::BACK,
.row_hint = RowSelectionHint::MIDDLE,
.spot_hint = SpotSelectionHint::RIGHT,
},
{
- .index = arm::ScoreBackMidConeDownPosIndex(),
+ .index = arm::ScoreBackLowConeUpIndex(),
+ .wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_UP,
+ .buttons = {kLowConeScoreLeft, kLowConeScoreRight},
+ .side = Side::BACK,
+ .row_hint = RowSelectionHint::BOTTOM,
+ .spot_hint = SpotSelectionHint::LEFT,
+ },
+ {
+ .index = arm::ScoreFrontLowConeUpIndex(),
+ .wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_UP,
+ .buttons = {kLowConeScoreLeft, kLowConeScoreRight},
+ .side = Side::FRONT,
+ .row_hint = RowSelectionHint::BOTTOM,
+ .spot_hint = SpotSelectionHint::LEFT,
+ },
+ {
+ .index = arm::ScoreBackMidConeDownBaseIndex(),
.wrist_goal = 2.2,
.score_wrist_goal = 0.0,
.game_piece = GamePiece::CONE_DOWN,
- .buttons = {kMidConeScoreRight},
+ .buttons = {kMidConeScoreLeft, kMidConeScoreRight},
.side = Side::BACK,
.row_hint = RowSelectionHint::MIDDLE,
.spot_hint = SpotSelectionHint::RIGHT,
},
{
+ .index = arm::ScoreBackLowConeDownBaseIndex(),
+ .wrist_goal = 0.0,
+ .score_wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_DOWN,
+ .buttons = {kLowConeScoreLeft, kLowConeScoreRight},
+ .side = Side::BACK,
+ .row_hint = RowSelectionHint::BOTTOM,
+ .spot_hint = SpotSelectionHint::RIGHT,
+ },
+ {
+ .index = arm::ScoreFrontLowConeDownBaseIndex(),
+ .wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_DOWN,
+ .buttons = {kLowConeScoreLeft, kLowConeScoreRight},
+ .side = Side::FRONT,
+ .row_hint = RowSelectionHint::BOTTOM,
+ .spot_hint = SpotSelectionHint::LEFT,
+ },
+ {
+ .index = arm::ScoreFrontMidConeDownBaseIndex(),
+ .wrist_goal = 2.0,
+ .score_wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_DOWN,
+ .buttons = {kMidConeScoreLeft, kMidConeScoreRight},
+ .side = Side::FRONT,
+ .row_hint = RowSelectionHint::MIDDLE,
+ .spot_hint = SpotSelectionHint::LEFT,
+ },
+ {
+ .index = arm::ScoreFrontHighConeDownBaseIndex(),
+ .wrist_goal = 2.0,
+ .score_wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_DOWN,
+ .buttons = {kHighConeScoreLeft, kHighConeScoreRight},
+ .side = Side::FRONT,
+ .row_hint = RowSelectionHint::TOP,
+ .spot_hint = SpotSelectionHint::LEFT,
+ },
+ {
+ .index = arm::HPPickupFrontConeUpIndex(),
+ .wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_UP,
+ .buttons = {kHPConePickup},
+ .side = Side::FRONT,
+ },
+ {
.index = arm::HPPickupBackConeUpIndex(),
- .wrist_goal = 0.2,
+ .wrist_goal = 0.4,
.game_piece = GamePiece::CONE_UP,
.buttons = {kHPConePickup},
.side = Side::BACK,
},
{
- .index = arm::ScoreFrontHighConeUpPosIndex(),
+ .index = arm::ScoreFrontHighConeUpIndex(),
.wrist_goal = 0.05,
.game_piece = GamePiece::CONE_UP,
.buttons = {kHighConeScoreLeft, kHighConeScoreRight},
@@ -141,7 +222,7 @@
.spot_hint = SpotSelectionHint::LEFT,
},
{
- .index = arm::ScoreFrontMidConeUpPosIndex(),
+ .index = arm::ScoreFrontMidConeUpIndex(),
.wrist_goal = 0.05,
.game_piece = GamePiece::CONE_UP,
.buttons = {kMidConeScoreLeft, kMidConeScoreRight},
@@ -261,7 +342,7 @@
if (data.IsPressed(kGroundPickupConeUp) || data.IsPressed(kHPConePickup)) {
roller_goal = RollerGoal::INTAKE_CONE;
current_game_piece_ = GamePiece::CONE_UP;
- } else if (data.IsPressed(kGroundPickupConeDown)) {
+ } else if (data.IsPressed(kGroundPickupConeDownBase)) {
roller_goal = RollerGoal::INTAKE_CONE;
current_game_piece_ = GamePiece::CONE_DOWN;
} else if (data.IsPressed(kGroundPickupCube)) {
@@ -273,26 +354,49 @@
wrist_goal = 0.6;
}
- const Side current_side = data.IsPressed(kBack) ? Side::BACK : Side::FRONT;
std::optional<RowSelectionHint> placing_row;
std::optional<SpotSelectionHint> placing_spot;
- // Search for the active setpoint.
- for (const ArmSetpoint &setpoint : setpoints) {
- for (const ButtonLocation &button : setpoint.buttons) {
+ // Keep the setpoint if the button is still held. This lets us release the
+ // back button once a side has been selected.
+ if (current_setpoint_ != nullptr) {
+ bool found = false;
+ for (const ButtonLocation &button : current_setpoint_->buttons) {
if (data.IsPressed(button)) {
- if (setpoint.game_piece == current_game_piece_ &&
- setpoint.side == current_side) {
- wrist_goal = setpoint.wrist_goal;
- arm_goal_position_ = setpoint.index;
- score_wrist_goal = setpoint.score_wrist_goal;
- placing_row = setpoint.row_hint;
- placing_spot = setpoint.spot_hint;
- break;
+ found = true;
+ }
+ }
+ if (!found) {
+ current_setpoint_ = nullptr;
+ }
+ }
+
+ // Ok, no active setpoint. Search for the right one.
+ if (current_setpoint_ == nullptr) {
+ const Side current_side =
+ data.IsPressed(kBack) ? Side::BACK : Side::FRONT;
+ // Search for the active setpoint.
+ for (const ArmSetpoint &setpoint : setpoints) {
+ for (const ButtonLocation &button : setpoint.buttons) {
+ if (data.IsPressed(button)) {
+ if (setpoint.game_piece == current_game_piece_ &&
+ setpoint.side == current_side) {
+ current_setpoint_ = &setpoint;
+ }
}
}
}
}
+
+ // And, pull the bits out of it.
+ if (current_setpoint_ != nullptr) {
+ wrist_goal = current_setpoint_->wrist_goal;
+ arm_goal_position_ = current_setpoint_->index;
+ score_wrist_goal = current_setpoint_->score_wrist_goal;
+ placing_row = current_setpoint_->row_hint;
+ placing_spot = current_setpoint_->spot_hint;
+ }
+
CHECK_EQ(placing_row.has_value(), placing_spot.has_value());
if (data.IsPressed(kSuck)) {
@@ -351,6 +455,8 @@
::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
uint32_t arm_goal_position_;
+
+ const ArmSetpoint *current_setpoint_ = nullptr;
};
} // namespace joysticks