Add human player station to target selector
We may need to tune the positions of the pickup, since it depends on
where the human player slides the tray to.
Change-Id: I881f42f60647d04a09ff1fcb5db8e6eec189b93b
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index 1b70ca1..0fb5df5 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -1,6 +1,5 @@
#include "y2023/control_loops/drivetrain/target_selector.h"
-#include "aos/containers/sized_array.h"
#include "frc971/shooter_interpolation/interpolation.h"
#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
#include "y2023/vision/game_pieces_generated.h"
@@ -53,6 +52,74 @@
}
}
+aos::SizedArray<const frc971::vision::Position *, 3>
+TargetSelector::PossibleScoringPositions(
+ const TargetSelectorHint *hint, const localizer::HalfField *scoring_map) {
+ aos::SizedArray<const localizer::ScoringGrid *, 3> possible_grids;
+ if (hint->has_grid()) {
+ possible_grids = {[hint, scoring_map]() -> const localizer::ScoringGrid * {
+ switch (hint->grid()) {
+ case GridSelectionHint::LEFT:
+ return scoring_map->left_grid();
+ case GridSelectionHint::MIDDLE:
+ return scoring_map->middle_grid();
+ case GridSelectionHint::RIGHT:
+ return scoring_map->right_grid();
+ }
+ // Make roborio compiler happy...
+ return nullptr;
+ }()};
+ } else {
+ possible_grids = {scoring_map->left_grid(), scoring_map->middle_grid(),
+ scoring_map->right_grid()};
+ }
+
+ aos::SizedArray<const localizer::ScoringRow *, 3> possible_rows =
+ [possible_grids, hint]() {
+ aos::SizedArray<const localizer::ScoringRow *, 3> rows;
+ for (const localizer::ScoringGrid *grid : possible_grids) {
+ CHECK_NOTNULL(grid);
+ switch (hint->row()) {
+ case RowSelectionHint::BOTTOM:
+ rows.push_back(grid->bottom());
+ break;
+ case RowSelectionHint::MIDDLE:
+ rows.push_back(grid->middle());
+ break;
+ case RowSelectionHint::TOP:
+ rows.push_back(grid->top());
+ break;
+ }
+ }
+ return rows;
+ }();
+ aos::SizedArray<const frc971::vision::Position *, 3> positions;
+ for (const localizer::ScoringRow *row : possible_rows) {
+ CHECK_NOTNULL(row);
+ switch (hint->spot()) {
+ case SpotSelectionHint::LEFT:
+ positions.push_back(row->left_cone());
+ break;
+ case SpotSelectionHint::MIDDLE:
+ positions.push_back(row->cube());
+ break;
+ case SpotSelectionHint::RIGHT:
+ positions.push_back(row->right_cone());
+ break;
+ }
+ }
+ return positions;
+}
+
+aos::SizedArray<const frc971::vision::Position *, 3>
+TargetSelector::PossiblePickupPositions(
+ const localizer::HalfField *scoring_map) {
+ aos::SizedArray<const frc971::vision::Position *, 3> positions;
+ positions.push_back(scoring_map->substation()->left());
+ positions.push_back(scoring_map->substation()->right());
+ return positions;
+}
+
bool TargetSelector::UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
double /*command_speed*/) {
UpdateAlliance();
@@ -77,63 +144,11 @@
}
last_hint_ = hint_object;
}
- aos::SizedArray<const localizer::ScoringGrid *, 3> possible_grids;
- if (hint_fetcher_->has_grid()) {
- possible_grids = {[this]() -> const localizer::ScoringGrid * {
- switch (hint_fetcher_->grid()) {
- case GridSelectionHint::LEFT:
- return scoring_map_->left_grid();
- case GridSelectionHint::MIDDLE:
- return scoring_map_->middle_grid();
- case GridSelectionHint::RIGHT:
- return scoring_map_->right_grid();
- }
- // Make roborio compiler happy...
- return nullptr;
- }()};
- } else {
- possible_grids = {scoring_map_->left_grid(), scoring_map_->middle_grid(),
- scoring_map_->right_grid()};
- }
-
- aos::SizedArray<const localizer::ScoringRow *, 3> possible_rows =
- [this, possible_grids]() {
- aos::SizedArray<const localizer::ScoringRow *, 3> rows;
- for (const localizer::ScoringGrid *grid : possible_grids) {
- CHECK_NOTNULL(grid);
- switch (hint_fetcher_->row()) {
- case RowSelectionHint::BOTTOM:
- rows.push_back(grid->bottom());
- break;
- case RowSelectionHint::MIDDLE:
- rows.push_back(grid->middle());
- break;
- case RowSelectionHint::TOP:
- rows.push_back(grid->top());
- break;
- }
- }
- return rows;
- }();
- aos::SizedArray<const frc971::vision::Position *, 3> possible_positions =
- [this, possible_rows]() {
- aos::SizedArray<const frc971::vision::Position *, 3> positions;
- for (const localizer::ScoringRow *row : possible_rows) {
- CHECK_NOTNULL(row);
- switch (hint_fetcher_->spot()) {
- case SpotSelectionHint::LEFT:
- positions.push_back(row->left_cone());
- break;
- case SpotSelectionHint::MIDDLE:
- positions.push_back(row->cube());
- break;
- case SpotSelectionHint::RIGHT:
- positions.push_back(row->right_cone());
- break;
- }
- }
- return positions;
- }();
+ const aos::SizedArray<const frc971::vision::Position *, 3>
+ possible_positions =
+ hint_fetcher_->substation_pickup()
+ ? PossiblePickupPositions(scoring_map_)
+ : PossibleScoringPositions(hint_fetcher_.get(), scoring_map_);
CHECK_LT(0u, possible_positions.size());
aos::SizedArray<double, 3> distances;
std::optional<double> closest_distance;
diff --git a/y2023/control_loops/drivetrain/target_selector.h b/y2023/control_loops/drivetrain/target_selector.h
index 5e7f015..e469ce9 100644
--- a/y2023/control_loops/drivetrain/target_selector.h
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -1,5 +1,6 @@
#ifndef Y2023_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
#define Y2023_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
+#include "aos/containers/sized_array.h"
#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/drivetrain/localizer.h"
#include "frc971/control_loops/pose.h"
@@ -47,6 +48,11 @@
private:
void UpdateAlliance();
+ static aos::SizedArray<const frc971::vision::Position *, 3>
+ PossibleScoringPositions(const TargetSelectorHint *hint,
+ const localizer::HalfField *scoring_map);
+ static aos::SizedArray<const frc971::vision::Position *, 3>
+ PossiblePickupPositions(const localizer::HalfField *scoring_map);
std::optional<Pose> target_pose_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<TargetSelectorHint> hint_fetcher_;
diff --git a/y2023/control_loops/drivetrain/target_selector_hint.fbs b/y2023/control_loops/drivetrain/target_selector_hint.fbs
index 357bc21..ce5fd89 100644
--- a/y2023/control_loops/drivetrain/target_selector_hint.fbs
+++ b/y2023/control_loops/drivetrain/target_selector_hint.fbs
@@ -30,7 +30,8 @@
row:RowSelectionHint (id: 1);
spot:SpotSelectionHint (id: 2);
robot_side:frc971.control_loops.drivetrain.RobotSide = DONT_CARE (id: 3);
- // TODO: support human player pickup auto-align?
+ // If set, attempt to pickup from the human player station.
+ substation_pickup:bool (id: 4);
}
root_type TargetSelectorHint;
diff --git a/y2023/control_loops/drivetrain/target_selector_test.cc b/y2023/control_loops/drivetrain/target_selector_test.cc
index c28c14d..21f3fe6 100644
--- a/y2023/control_loops/drivetrain/target_selector_test.cc
+++ b/y2023/control_loops/drivetrain/target_selector_test.cc
@@ -55,6 +55,14 @@
builder.CheckOk(builder.Send(hint_builder.Finish()));
}
+ void SendSubstationHint() {
+ auto builder = hint_sender_.MakeBuilder();
+ TargetSelectorHint::Builder hint_builder =
+ builder.MakeBuilder<TargetSelectorHint>();
+ hint_builder.add_substation_pickup(true);
+ builder.CheckOk(builder.Send(hint_builder.Finish()));
+ }
+
const localizer::HalfField *scoring_map() const {
return constants_fetcher_.constants().scoring_map()->red();
}
@@ -185,4 +193,29 @@
EXPECT_EQ(target.y(), middle_pos->y());
}
+// Test that substation pickup being set in the hint causes us to pickup from
+// the substation.
+TEST_F(TargetSelectorTest, SubstationPickup) {
+ SendJoystickState();
+ SendSubstationHint();
+ const frc971::vision::Position *left_pos =
+ scoring_map()->substation()->left();
+ const frc971::vision::Position *right_pos =
+ scoring_map()->substation()->right();
+ Eigen::Matrix<double, 5, 1> left_position;
+ left_position << 0.0, left_pos->y(), 0.0, 0.0, 0.0;
+ Eigen::Matrix<double, 5, 1> right_position;
+ right_position << 0.0, right_pos->y(), 0.0, 0.0, 0.0;
+
+ EXPECT_TRUE(target_selector_.UpdateSelection(left_position, 0.0));
+ Eigen::Vector3d target = target_selector_.TargetPose().abs_pos();
+ EXPECT_EQ(target.x(), left_pos->x());
+ EXPECT_EQ(target.y(), left_pos->y());
+
+ EXPECT_TRUE(target_selector_.UpdateSelection(right_position, 0.0));
+ target = target_selector_.TargetPose().abs_pos();
+ EXPECT_EQ(target.x(), right_pos->x());
+ EXPECT_EQ(target.y(), right_pos->y());
+}
+
} // namespace y2023::control_loops::drivetrain
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 4c4b1e3..3951dfa 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -434,7 +434,17 @@
AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
- if (placing_row.has_value()) {
+ // TODO(james): Is there a more principled way to detect Human Player
+ // pickup? Probably don't bother fixing it until/unless we add more buttons
+ // that can select human player pickup.
+ if (data.IsPressed(kHPConePickup)) {
+ auto builder = target_selector_hint_sender_.MakeBuilder();
+ auto hint_builder = builder.MakeBuilder<TargetSelectorHint>();
+ hint_builder.add_substation_pickup(true);
+ if (builder.Send(hint_builder.Finish()) != aos::RawSender::Error::kOk) {
+ AOS_LOG(ERROR, "Sending target selector hint failed.\n");
+ }
+ } else if (placing_row.has_value()) {
auto builder = target_selector_hint_sender_.MakeBuilder();
auto hint_builder = builder.MakeBuilder<TargetSelectorHint>();
hint_builder.add_row(placing_row.value());