Support selecting drivetrain side in TargetSelector
In doing this, I am thinking that we'll probably want to make the
hysteresis in the target selector *much* less aggressive so that we can
allow the manipulator to make mistakes.
Change-Id: I7c5047f75e7e0fd4eae168e1a40ffbb71c093f87
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index 32b8c15..a12326c 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -131,6 +131,9 @@
],
gen_reflections = 1,
visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ ],
)
cc_library(
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index 99805cb..e49ba2f 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -115,6 +115,12 @@
}
CHECK(closest_position.has_value());
target_pose_ = Pose(closest_position.value(), /*theta=*/0.0);
+ if (hint_fetcher_->has_robot_side()) {
+ drive_direction_ = hint_fetcher_->robot_side();
+ } else {
+ drive_direction_ = Side::DONT_CARE;
+ }
return true;
}
+
} // namespace y2023::control_loops::drivetrain
diff --git a/y2023/control_loops/drivetrain/target_selector.h b/y2023/control_loops/drivetrain/target_selector.h
index 0290425..9c4600d 100644
--- a/y2023/control_loops/drivetrain/target_selector.h
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -23,6 +23,7 @@
: public frc971::control_loops::drivetrain::TargetSelectorInterface {
public:
typedef frc971::control_loops::TypedPose<double> Pose;
+ typedef frc971::control_loops::drivetrain::RobotSide Side;
TargetSelector(aos::EventLoop *event_loop);
@@ -36,6 +37,7 @@
}
double TargetRadius() const override { return 0.0; }
+ Side DriveDirection() const override { return drive_direction_; }
private:
void UpdateAlliance();
@@ -44,6 +46,7 @@
aos::Fetcher<TargetSelectorHint> hint_fetcher_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
const localizer::HalfField *scoring_map_ = nullptr;
+ Side drive_direction_ = Side::DONT_CARE;
};
} // namespace y2023::control_loops::drivetrain
#endif // Y2023_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
diff --git a/y2023/control_loops/drivetrain/target_selector_hint.fbs b/y2023/control_loops/drivetrain/target_selector_hint.fbs
index 518d302..399d772 100644
--- a/y2023/control_loops/drivetrain/target_selector_hint.fbs
+++ b/y2023/control_loops/drivetrain/target_selector_hint.fbs
@@ -1,3 +1,5 @@
+include "frc971/control_loops/drivetrain/drivetrain_status.fbs";
+
namespace y2023.control_loops.drivetrain;
// Which of the grids we are going for.
@@ -23,13 +25,12 @@
RIGHT,
}
-
table TargetSelectorHint {
grid:GridSelectionHint (id: 0);
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?
- // TODO: add flag for forwards vs. backwards.
}
root_type TargetSelectorHint;
diff --git a/y2023/control_loops/drivetrain/target_selector_test.cc b/y2023/control_loops/drivetrain/target_selector_test.cc
index fdbd904..fbe5e8d 100644
--- a/y2023/control_loops/drivetrain/target_selector_test.cc
+++ b/y2023/control_loops/drivetrain/target_selector_test.cc
@@ -4,6 +4,8 @@
#include "gtest/gtest.h"
#include "y2023/constants/simulated_constants_sender.h"
+using Side = frc971::control_loops::drivetrain::RobotSide;
+
namespace y2023::control_loops::drivetrain {
class TargetSelectorTest : public ::testing::Test {
protected:
@@ -38,17 +40,18 @@
}
void SendHint(GridSelectionHint grid, RowSelectionHint row,
- SpotSelectionHint spot) {
+ SpotSelectionHint spot, Side side) {
auto builder = hint_sender_.MakeBuilder();
builder.CheckOk(builder.Send(
- CreateTargetSelectorHint(*builder.fbb(), grid, row, spot)));
+ CreateTargetSelectorHint(*builder.fbb(), grid, row, spot, side)));
}
- void SendHint(RowSelectionHint row, SpotSelectionHint spot) {
+ void SendHint(RowSelectionHint row, SpotSelectionHint spot, Side side) {
auto builder = hint_sender_.MakeBuilder();
TargetSelectorHint::Builder hint_builder =
builder.MakeBuilder<TargetSelectorHint>();
hint_builder.add_row(row);
hint_builder.add_spot(spot);
+ hint_builder.add_robot_side(side);
builder.CheckOk(builder.Send(hint_builder.Finish()));
}
@@ -95,13 +98,16 @@
{row->left_cone(), SpotSelectionHint::LEFT},
{row->cube(), SpotSelectionHint::MIDDLE},
{row->right_cone(), SpotSelectionHint::RIGHT}}) {
- SendHint(grid_hint, row_hint, spot_hint);
+ SendHint(grid_hint, row_hint, spot_hint, Side::FRONT);
EXPECT_TRUE(target_selector_.UpdateSelection(
Eigen::Matrix<double, 5, 1>::Zero(), 0.0));
EXPECT_EQ(0.0, target_selector_.TargetRadius());
EXPECT_EQ(spot->x(), target_selector_.TargetPose().abs_pos().x());
EXPECT_EQ(spot->y(), target_selector_.TargetPose().abs_pos().y());
EXPECT_EQ(spot->z(), target_selector_.TargetPose().abs_pos().z());
+ EXPECT_EQ(frc971::control_loops::drivetrain::TargetSelectorInterface::
+ Side::FRONT,
+ target_selector_.DriveDirection());
}
}
}
@@ -125,12 +131,15 @@
SpotSelectionHint::MIDDLE},
{scoring_map()->left_grid()->bottom()->right_cone(),
SpotSelectionHint::RIGHT}}) {
- SendHint(RowSelectionHint::BOTTOM, spot_hint);
+ SendHint(RowSelectionHint::BOTTOM, spot_hint, Side::BACK);
EXPECT_TRUE(target_selector_.UpdateSelection(
Eigen::Matrix<double, 5, 1>::Zero(), 0.0));
EXPECT_EQ(spot->x(), target_selector_.TargetPose().abs_pos().x());
EXPECT_EQ(spot->y(), target_selector_.TargetPose().abs_pos().y());
EXPECT_EQ(spot->z(), target_selector_.TargetPose().abs_pos().z());
+ EXPECT_EQ(
+ frc971::control_loops::drivetrain::TargetSelectorInterface::Side::BACK,
+ target_selector_.DriveDirection());
}
}