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
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 2d43d99..e2b8a34 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -510,6 +510,7 @@
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -530,6 +531,7 @@
goal_builder.add_wrist(wrist_offset);
+ goal_builder.add_arm_goal_position(arm::NeutralPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -752,7 +754,7 @@
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::PickupPosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -778,7 +780,7 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::PickupPosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -795,7 +797,7 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::PickupPosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreBackMidConeUpPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -806,7 +808,7 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_arm_goal_position(arm::ScorePosIndex());
+ goal_builder.add_arm_goal_position(arm::ScoreLowPosIndex());
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 112c209..58f7df1 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -41,6 +41,14 @@
const ButtonLocation kScore(4, 4);
const ButtonLocation kSpit(4, 13);
+const ButtonLocation kMidBackTipConeScoreLeft(4, 15);
+const ButtonLocation kHighBackTipConeScoreLeft(4, 14);
+const ButtonLocation kMidBackTipConeScoreRight(3, 2);
+
+const ButtonLocation kGroundPickupConeUp(4, 7);
+const ButtonLocation kGroundPickupConeDown(4, 8);
+const ButtonLocation kHPConePickup(4, 6);
+
const ButtonLocation kSuck(4, 12);
const ButtonLocation kWrist(4, 10);
@@ -48,6 +56,66 @@
namespace superstructure = y2023::control_loops::superstructure;
namespace arm = superstructure::arm;
+enum class GamePiece {
+ CONE_UP = 0,
+ CONE_DOWN = 1,
+ CUBE = 2,
+};
+
+struct ArmSetpoint {
+ uint32_t index;
+ double wrist_goal;
+ std::optional<double> score_wrist_goal = std::nullopt;
+ GamePiece game_piece;
+ ButtonLocation button;
+};
+
+const std::vector<ArmSetpoint> setpoints = {
+ {
+ .index = arm::GroundPickupBackConeUpIndex(),
+ .wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_UP,
+ .button = kGroundPickupConeUp,
+ },
+ {
+ .index = arm::GroundPickupBackConeDownIndex(),
+ .wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_DOWN,
+ .button = kGroundPickupConeDown,
+ },
+ {
+ .index = arm::ScoreBackMidConeUpPosIndex(),
+ .wrist_goal = 0.55,
+ .game_piece = GamePiece::CONE_UP,
+ .button = kMidBackTipConeScoreRight,
+ },
+ {
+ .index = arm::ScoreBackMidConeDownPosIndex(),
+ .wrist_goal = 2.2,
+ .score_wrist_goal = 0.0,
+ .game_piece = GamePiece::CONE_DOWN,
+ .button = kMidBackTipConeScoreRight,
+ },
+ {
+ .index = arm::HPPickupBackConeUpIndex(),
+ .wrist_goal = 0.2,
+ .game_piece = GamePiece::CONE_UP,
+ .button = kHPConePickup,
+ },
+ {
+ .index = arm::ScoreFrontHighConeUpPosIndex(),
+ .wrist_goal = 0.05,
+ .game_piece = GamePiece::CONE_UP,
+ .button = kHighBackTipConeScoreLeft,
+ },
+ {
+ .index = arm::ScoreFrontMidConeUpPosIndex(),
+ .wrist_goal = 0.05,
+ .game_piece = GamePiece::CONE_UP,
+ .button = kMidBackTipConeScoreLeft,
+ },
+};
+
class Reader : public ::frc971::input::ActionJoystickInput {
public:
Reader(::aos::EventLoop *event_loop)
@@ -63,6 +131,8 @@
void AutoEnded() override { AOS_LOG(INFO, "Auto ended.\n"); }
+ GamePiece current_game_piece_ = GamePiece::CONE_UP;
+
void HandleTeleop(
const ::frc971::input::driver_station::Data &data) override {
superstructure_status_fetcher_.Fetch();
@@ -71,39 +141,61 @@
return;
}
- RollerGoal roller_goal = RollerGoal::IDLE;
+ if (!superstructure_status_fetcher_->has_wrist()) {
+ AOS_LOG(ERROR, "Got no superstructure status message.\n");
+ return;
+ }
- // TODO(milind): add more actions and paths
- if (data.IsPressed(kIntake)) {
- arm_goal_position_ = arm::ScorePosIndex();
- } else if (data.IsPressed(kScore)) {
- arm_goal_position_ = arm::ScorePosIndex();
- } else {
- arm_goal_position_ = arm::NeutralPosIndex();
+ double wrist_goal = 0.0;
+ RollerGoal roller_goal = RollerGoal::IDLE;
+ arm_goal_position_ = arm::NeutralPosIndex();
+ std::optional<double> score_wrist_goal = std::nullopt;
+
+ if (data.IsPressed(kGroundPickupConeUp) || data.IsPressed(kHPConePickup)) {
+ roller_goal = RollerGoal::INTAKE;
+ current_game_piece_ = GamePiece::CONE_UP;
+ } else if (data.IsPressed(kGroundPickupConeDown)) {
+ roller_goal = RollerGoal::INTAKE;
+ current_game_piece_ = GamePiece::CONE_DOWN;
+ }
+
+ // Search for the active setpoint.
+ for (const ArmSetpoint &setpoint : setpoints) {
+ if (data.IsPressed(setpoint.button)) {
+ if (setpoint.game_piece == current_game_piece_) {
+ wrist_goal = setpoint.wrist_goal;
+ arm_goal_position_ = setpoint.index;
+ score_wrist_goal = setpoint.score_wrist_goal;
+ break;
+ }
+ }
}
if (data.IsPressed(kSuck)) {
roller_goal = RollerGoal::INTAKE;
} else if (data.IsPressed(kSpit)) {
- roller_goal = RollerGoal::SPIT;
- }
+ if (score_wrist_goal.has_value()) {
+ wrist_goal = score_wrist_goal.value();
- double wrist_goal = 0.1;
-
- if (data.IsPressed(kWrist)) {
- wrist_goal = 1.5;
- } else {
- wrist_goal = 0.1;
+ // If we are supposed to dunk it, wait until we are close enough to
+ // spit.
+ if (std::abs(score_wrist_goal.value() -
+ superstructure_status_fetcher_->wrist()->position()) <
+ 0.1) {
+ roller_goal = RollerGoal::SPIT;
+ }
+ } else {
+ roller_goal = RollerGoal::SPIT;
+ }
}
{
auto builder = superstructure_goal_sender_.MakeBuilder();
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
- wrist_offset =
- CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), wrist_goal,
- CreateProfileParameters(*builder.fbb(), 12.0, 90.0));
+ wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), wrist_goal,
+ CreateProfileParameters(*builder.fbb(), 12.0, 90.0));
superstructure::Goal::Builder superstructure_goal_builder =
builder.MakeBuilder<superstructure::Goal>();