Provide 2023 target selector
This should let us do the exact same logic as y2019. This means that the
driver has to control drivetrain velocity directly (not, e.g.,
position), but is a logical first step.
This has an extremely initial tuning that should work sanely.
Does not support going backwards yet.
Change-Id: I29675be6e6d73106563f4abc6e823a1ffcb39946
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 9150e8a..51f92b1 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -49,6 +49,31 @@
int do_accel_corrections = 50;
};
+// Configuration for line-following mode.
+struct LineFollowConfig {
+ // The line-following uses an LQR controller with states of [theta,
+ // linear_velocity, angular_velocity] and inputs of [left_voltage,
+ // right_voltage].
+ // These Q and R matrices are the costs for state and input respectively.
+ Eigen::Matrix3d Q =
+ Eigen::Matrix3d((::Eigen::DiagonalMatrix<double, 3>().diagonal()
+ << 1.0 / ::std::pow(0.1, 2),
+ 1.0 / ::std::pow(1.0, 2), 1.0 / ::std::pow(1.0, 2))
+ .finished()
+ .asDiagonal());
+ Eigen::Matrix2d R =
+ Eigen::Matrix2d((::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 1.0 / ::std::pow(12.0, 2),
+ 1.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal());
+
+ // The driver can use their steering controller to adjust where we attempt to
+ // place things laterally. This specifies how much range on either side of
+ // zero we allow them, in meters.
+ double max_controllable_offset = 0.1;
+};
+
template <typename Scalar = double>
struct DrivetrainConfig {
// Shifting method we are using.
@@ -118,6 +143,8 @@
DownEstimatorConfig down_estimator_config{};
+ LineFollowConfig line_follow_config{};
+
// Converts the robot state to a linear distance position, velocity.
static Eigen::Matrix<Scalar, 2, 1> LeftRightToLinear(
const Eigen::Matrix<Scalar, 7, 1> &left_right) {
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index a53538b..95f14a3 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -95,6 +95,8 @@
const DrivetrainConfig<double> &dt_config,
TargetSelectorInterface *target_selector)
: dt_config_(dt_config),
+ Q_(dt_config_.line_follow_config.Q),
+ R_(dt_config_.line_follow_config.R),
A_d_(ADiscrete(dt_config_)),
B_d_(BDiscrete(dt_config_)),
K_(CalcK(A_d_, B_d_, Q_, R_)),
@@ -131,7 +133,8 @@
}
// Set an adjustment that lets the driver tweak the offset for where we place
// the target left/right.
- side_adjust_ = -goal->wheel() * 0.1;
+ side_adjust_ =
+ -goal->wheel() * dt_config_.line_follow_config.max_controllable_offset;
}
bool LineFollowDrivetrain::SetOutput(
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.h b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
index 24f8cd4..c5b03f0 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.h
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.h
@@ -1,7 +1,6 @@
#ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
#define FRC971_CONTROL_LOOPS_DRIVETRAIN_LINE_FOLLOW_DRIVETRAIN_H_
#include "Eigen/Dense"
-
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
@@ -56,21 +55,10 @@
double relative_y_offset, double velocity_sign);
const DrivetrainConfig<double> dt_config_;
- // TODO(james): These should probably be factored out somewhere.
// TODO(james): This controller is not actually asymptotically stable, due to
// the varying goal theta.
- const ::Eigen::DiagonalMatrix<double, 3> Q_ =
- (::Eigen::DiagonalMatrix<double, 3>().diagonal()
- << 1.0 / ::std::pow(0.1, 2),
- 1.0 / ::std::pow(1.0, 2), 1.0 / ::std::pow(1.0, 2))
- .finished()
- .asDiagonal();
- const ::Eigen::DiagonalMatrix<double, 2> R_ =
- (::Eigen::DiagonalMatrix<double, 2>().diagonal()
- << 1.0 / ::std::pow(12.0, 2),
- 1.0 / ::std::pow(12.0, 2))
- .finished()
- .asDiagonal();
+ const ::Eigen::Matrix<double, 3, 3> Q_;
+ const ::Eigen::Matrix<double, 2, 2> R_;
// The matrices we use for the linear controller.
// State for these is [theta, linear_velocity, angular_velocity]
const ::Eigen::Matrix<double, 3, 3> A_d_;
diff --git a/frc971/control_loops/drivetrain/localization/puppet_localizer.cc b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
index 3125793..33095f2 100644
--- a/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.cc
@@ -7,7 +7,9 @@
PuppetLocalizer::PuppetLocalizer(
aos::EventLoop *event_loop,
const frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config)
+ &dt_config,
+ std::unique_ptr<frc971::control_loops::drivetrain::TargetSelectorInterface>
+ target_selector)
: event_loop_(event_loop),
dt_config_(dt_config),
ekf_(dt_config),
@@ -17,7 +19,8 @@
"/localizer")),
clock_offset_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
- "/aos")) {
+ "/aos")),
+ target_selector_(std::move(target_selector)) {
ekf_.set_ignore_accel(true);
event_loop->OnRun([this, event_loop]() {
@@ -25,7 +28,11 @@
HybridEkf::State::Zero(), ekf_.P());
});
- target_selector_.set_has_target(false);
+ if (!target_selector_) {
+ auto selector = std::make_unique<TrivialTargetSelector>();
+ selector->set_has_target(false);
+ target_selector_ = std::move(selector);
+ }
}
void PuppetLocalizer::Reset(
diff --git a/frc971/control_loops/drivetrain/localization/puppet_localizer.h b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
index 4f8f4f3..24e2f88 100644
--- a/frc971/control_loops/drivetrain/localization/puppet_localizer.h
+++ b/frc971/control_loops/drivetrain/localization/puppet_localizer.h
@@ -32,14 +32,17 @@
PuppetLocalizer(
aos::EventLoop *event_loop,
const frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config);
+ &dt_config,
+ std::unique_ptr<
+ frc971::control_loops::drivetrain::TargetSelectorInterface>
+ target_selector = {});
frc971::control_loops::drivetrain::HybridEkf<double>::State Xhat()
const override {
return ekf_.X_hat().cast<double>();
}
- frc971::control_loops::drivetrain::TrivialTargetSelector *target_selector()
+ frc971::control_loops::drivetrain::TargetSelectorInterface *target_selector()
override {
- return &target_selector_;
+ return target_selector_.get();
}
void Update(const ::Eigen::Matrix<double, 2, 1> &U,
@@ -97,7 +100,8 @@
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
// Target selector to allow us to satisfy the LocalizerInterface requirements.
- frc971::control_loops::drivetrain::TrivialTargetSelector target_selector_;
+ std::unique_ptr<frc971::control_loops::drivetrain::TargetSelectorInterface>
+ target_selector_;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 67d681e..d590f1d 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -15,6 +15,7 @@
// state updates and then determine what poes we should be driving to.
class TargetSelectorInterface {
public:
+ virtual ~TargetSelectorInterface() {}
// Take the state as [x, y, theta, left_vel, right_vel]
// If unable to determine what target to go for, returns false. If a viable
// target is selected, then returns true and sets target_pose.
@@ -108,6 +109,7 @@
// manually set the target selector state.
class TrivialTargetSelector : public TargetSelectorInterface {
public:
+ virtual ~TrivialTargetSelector() {}
bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &, double) override {
return has_target_;
}
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index fdc183f..19ac52e 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -32,6 +32,7 @@
/*num_obstacles=*/0, double> FakeCamera;
TargetSelector(::aos::EventLoop *event_loop);
+ virtual ~TargetSelector() {}
bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
double command_speed) override;
diff --git a/y2023/constants/test_data/scoring_map.json b/y2023/constants/test_data/scoring_map.json
new file mode 100644
index 0000000..ebb9b38
--- /dev/null
+++ b/y2023/constants/test_data/scoring_map.json
@@ -0,0 +1,348 @@
+{
+ "red": {
+ "substation": {
+ "left": {
+ "x": -8.068,
+ "y": 1.74,
+ "z": 0.95
+ },
+ "right": {
+ "x": -8.068,
+ "y": 3.74,
+ "z": 0.95
+ }
+ },
+ "left_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": 0.973,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": 0.414,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -0.145,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": 0.973,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": 0.414,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -0.145,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": 0.973,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": 0.414,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -0.145,
+ "z": 1.17
+ }
+ }
+ },
+ "middle_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": -0.703,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": -1.262,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -1.821,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": -0.703,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": -1.262,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -1.821,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": -0.703,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": -1.262,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -1.821,
+ "z": 1.17
+ }
+ }
+ },
+ "right_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": 6.99,
+ "y": -2.379,
+ "z": 0.0
+ },
+ "cube": {
+ "x": 6.99,
+ "y": -2.938,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": 6.99,
+ "y": -3.497,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": 7.461,
+ "y": -2.379,
+ "z": 0.87
+ },
+ "cube": {
+ "x": 7.461,
+ "y": -2.938,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": 7.461,
+ "y": -3.497,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": 7.891,
+ "y": -2.379,
+ "z": 1.17
+ },
+ "cube": {
+ "x": 7.891,
+ "y": -2.938,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": 7.891,
+ "y": -3.497,
+ "z": 1.17
+ }
+ }
+ }
+ },
+ "blue": {
+ "substation": {
+ "left": {
+ "x": 8.069,
+ "y": 3.74,
+ "z": 0.95
+ },
+ "right": {
+ "x": 8.069,
+ "y": 1.74,
+ "z": 0.95
+ }
+ },
+ "left_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -0.145,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": 0.414,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": 0.973,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -0.145,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": 0.414,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": 0.973,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -0.145,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": 0.414,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": 0.973,
+ "z": 1.17
+ }
+ }
+ },
+ "middle_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -1.821,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": -1.262,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": -0.703,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -1.821,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": -1.262,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": -0.703,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -1.821,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": -1.262,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": -0.703,
+ "z": 1.17
+ }
+ }
+ },
+ "right_grid": {
+ "bottom": {
+ "left_cone": {
+ "x": -6.989,
+ "y": -3.497,
+ "z": 0.0
+ },
+ "cube": {
+ "x": -6.989,
+ "y": -2.938,
+ "z": 0.0
+ },
+ "right_cone": {
+ "x": -6.989,
+ "y": -2.379,
+ "z": 0.0
+ }
+ },
+ "middle": {
+ "left_cone": {
+ "x": -7.46,
+ "y": -3.497,
+ "z": 0.87
+ },
+ "cube": {
+ "x": -7.46,
+ "y": -2.938,
+ "z": 0.87
+ },
+ "right_cone": {
+ "x": -7.46,
+ "y": -2.379,
+ "z": 0.87
+ }
+ },
+ "top": {
+ "left_cone": {
+ "x": -7.89,
+ "y": -3.497,
+ "z": 1.17
+ },
+ "cube": {
+ "x": -7.89,
+ "y": -2.938,
+ "z": 1.17
+ },
+ "right_cone": {
+ "x": -7.89,
+ "y": -2.379,
+ "z": 1.17
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/y2023/constants/test_data/test_team.json b/y2023/constants/test_data/test_team.json
index adc9eae..f09b23e 100644
--- a/y2023/constants/test_data/test_team.json
+++ b/y2023/constants/test_data/test_team.json
@@ -13,5 +13,6 @@
"calibration": {% include 'y2023/constants/test_data/calibration_pi-4.json' %}
}
],
- "target_map": {% include 'y2023/constants/test_data/target_map.json' %}
+ "target_map": {% include 'y2023/constants/test_data/target_map.json' %},
+ "scoring_map": {% include 'y2023/constants/test_data/scoring_map.json' %}
}
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index 416d6d9..32b8c15 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -81,6 +81,7 @@
visibility = ["//visibility:public"],
deps = [
":drivetrain_base",
+ ":target_selector",
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
@@ -131,3 +132,32 @@
gen_reflections = 1,
visibility = ["//visibility:public"],
)
+
+cc_library(
+ name = "target_selector",
+ srcs = ["target_selector.cc"],
+ hdrs = ["target_selector.h"],
+ deps = [
+ ":target_selector_hint_fbs",
+ "//aos/containers:sized_array",
+ "//aos/events:event_loop",
+ "//frc971/constants:constants_sender_lib",
+ "//frc971/control_loops:pose",
+ "//frc971/control_loops/drivetrain:localizer",
+ "//frc971/input:joystick_state_fbs",
+ "//y2023/constants:constants_fbs",
+ ],
+)
+
+cc_test(
+ name = "target_selector_test",
+ srcs = ["target_selector_test.cc"],
+ data = ["//y2023:aos_config"],
+ deps = [
+ ":target_selector",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ "//frc971/input:joystick_state_fbs",
+ "//y2023/constants:simulated_constants_sender",
+ ],
+)
diff --git a/y2023/control_loops/drivetrain/drivetrain_base.cc b/y2023/control_loops/drivetrain/drivetrain_base.cc
index cdb5a61..e8fdb3c 100644
--- a/y2023/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_base.cc
@@ -11,6 +11,7 @@
using ::frc971::control_loops::drivetrain::DownEstimatorConfig;
using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::LineFollowConfig;
namespace chrono = ::std::chrono;
@@ -61,7 +62,20 @@
.finished(),
false /*is_simulated*/,
DownEstimatorConfig{.gravity_threshold = 0.015,
- .do_accel_corrections = 1000}};
+ .do_accel_corrections = 1000},
+ LineFollowConfig{
+ .Q = Eigen::Matrix3d((::Eigen::DiagonalMatrix<double, 3>().diagonal()
+ << 1.0 / ::std::pow(0.1, 2),
+ 1.0 / ::std::pow(1.0, 2),
+ 1.0 / ::std::pow(1.0, 2))
+ .finished()
+ .asDiagonal()),
+ .R = Eigen::Matrix2d((::Eigen::DiagonalMatrix<double, 2>().diagonal()
+ << 10.0 / ::std::pow(12.0, 2),
+ 10.0 / ::std::pow(12.0, 2))
+ .finished()
+ .asDiagonal()),
+ .max_controllable_offset = 0.5}};
return kDrivetrainConfig;
};
diff --git a/y2023/control_loops/drivetrain/drivetrain_main.cc b/y2023/control_loops/drivetrain/drivetrain_main.cc
index 32b2455..b13e76f 100644
--- a/y2023/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2023/control_loops/drivetrain/drivetrain_main.cc
@@ -5,6 +5,7 @@
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "frc971/control_loops/drivetrain/localization/puppet_localizer.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
+#include "y2023/control_loops/drivetrain/target_selector.h"
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
@@ -19,7 +20,9 @@
localizer = std::make_unique<
::frc971::control_loops::drivetrain::PuppetLocalizer>(
&event_loop,
- ::y2023::control_loops::drivetrain::GetDrivetrainConfig());
+ ::y2023::control_loops::drivetrain::GetDrivetrainConfig(),
+ std::make_unique<::y2023::control_loops::drivetrain::TargetSelector>(
+ &event_loop));
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
y2023::control_loops::drivetrain::GetDrivetrainConfig(), &event_loop,
localizer.get());
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
new file mode 100644
index 0000000..99805cb
--- /dev/null
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -0,0 +1,120 @@
+#include "y2023/control_loops/drivetrain/target_selector.h"
+
+#include "aos/containers/sized_array.h"
+
+namespace y2023::control_loops::drivetrain {
+TargetSelector::TargetSelector(aos::EventLoop *event_loop)
+ : joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ hint_fetcher_(event_loop->MakeFetcher<TargetSelectorHint>("/drivetrain")),
+ constants_fetcher_(event_loop) {
+ CHECK(constants_fetcher_.constants().has_scoring_map());
+ CHECK(constants_fetcher_.constants().scoring_map()->has_red());
+ CHECK(constants_fetcher_.constants().scoring_map()->has_blue());
+}
+
+void TargetSelector::UpdateAlliance() {
+ joystick_state_fetcher_.Fetch();
+ if (joystick_state_fetcher_.get() != nullptr &&
+ joystick_state_fetcher_->has_alliance()) {
+ switch (joystick_state_fetcher_->alliance()) {
+ case aos::Alliance::kRed:
+ scoring_map_ = constants_fetcher_.constants().scoring_map()->red();
+ break;
+ case aos::Alliance::kBlue:
+ scoring_map_ = constants_fetcher_.constants().scoring_map()->blue();
+ break;
+ case aos::Alliance::kInvalid:
+ // Do nothing.
+ break;
+ }
+ }
+}
+
+bool TargetSelector::UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
+ double /*command_speed*/) {
+ UpdateAlliance();
+ if (scoring_map_ == nullptr) {
+ // We don't know which alliance we are on yet; wait on a JoystickState
+ // message.
+ return false;
+ }
+ hint_fetcher_.Fetch();
+ if (hint_fetcher_.get() == nullptr) {
+ // We don't know where to go, wait on a hint.
+ return false;
+ }
+ 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;
+ }();
+ CHECK_LT(0u, possible_positions.size());
+ std::optional<double> closest_distance;
+ std::optional<Eigen::Vector3d> closest_position;
+ const Eigen::Vector3d robot_position(state.x(), state.y(), 0.0);
+ for (const frc971::vision::Position *position : possible_positions) {
+ const Eigen::Vector3d target(position->x(), position->y(), position->z());
+ double distance = (target - robot_position).norm();
+ if (!closest_distance.has_value() || distance < closest_distance.value()) {
+ closest_distance = distance;
+ closest_position = target;
+ }
+ }
+ CHECK(closest_position.has_value());
+ target_pose_ = Pose(closest_position.value(), /*theta=*/0.0);
+ 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
new file mode 100644
index 0000000..0290425
--- /dev/null
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -0,0 +1,49 @@
+#ifndef Y2023_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
+#define Y2023_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
+#include "frc971/constants/constants_sender_lib.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/input/joystick_state_generated.h"
+#include "y2023/constants/constants_generated.h"
+#include "y2023/control_loops/drivetrain/target_selector_hint_generated.h"
+
+namespace y2023::control_loops::drivetrain {
+// This target selector provides the logic to choose which position to try to
+// guide the robot to (primarily for game piece placement; but also for game
+// piece pickup).
+// Currently, this works by:
+// 1. Relying on the constants + JoystickState message to figure out which set
+// of targets are relevant to us given the alliance that we are on).
+// 2. If the TargetSelectorHint message fully specifies where to score the game
+// piece, go there.
+// 3. If the exact grid to score in is unpopulated, score in the closest grid.
+// In the future, the code could readily be expanded to score in the nearest
+// valid position or resolve any other set of extra ambiguity.
+class TargetSelector
+ : public frc971::control_loops::drivetrain::TargetSelectorInterface {
+ public:
+ typedef frc971::control_loops::TypedPose<double> Pose;
+
+ TargetSelector(aos::EventLoop *event_loop);
+
+ bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
+ double command_speed) override;
+
+ Pose TargetPose() const override {
+ CHECK(target_pose_.has_value())
+ << "Did you check the return value of UpdateSelection()?";
+ return target_pose_.value();
+ }
+
+ double TargetRadius() const override { return 0.0; }
+
+ private:
+ void UpdateAlliance();
+ std::optional<Pose> target_pose_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+ aos::Fetcher<TargetSelectorHint> hint_fetcher_;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+ const localizer::HalfField *scoring_map_ = nullptr;
+};
+} // 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 0a16a59..518d302 100644
--- a/y2023/control_loops/drivetrain/target_selector_hint.fbs
+++ b/y2023/control_loops/drivetrain/target_selector_hint.fbs
@@ -28,6 +28,8 @@
grid:GridSelectionHint (id: 0);
row:RowSelectionHint (id: 1);
spot:SpotSelectionHint (id: 2);
+ // 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
new file mode 100644
index 0000000..fdbd904
--- /dev/null
+++ b/y2023/control_loops/drivetrain/target_selector_test.cc
@@ -0,0 +1,137 @@
+#include "y2023/control_loops/drivetrain/target_selector.h"
+
+#include "frc971/input/joystick_state_generated.h"
+#include "gtest/gtest.h"
+#include "y2023/constants/simulated_constants_sender.h"
+
+namespace y2023::control_loops::drivetrain {
+class TargetSelectorTest : public ::testing::Test {
+ protected:
+ TargetSelectorTest()
+ : configuration_(aos::configuration::ReadConfig("y2023/aos_config.json")),
+ event_loop_factory_(&configuration_.message()),
+ roborio_node_([this]() {
+ // Get the constants sent before anything else happens.
+ // It has nothing to do with the roborio node.
+ SendSimulationConstants(&event_loop_factory_, 7971,
+ "y2023/constants/test_constants.json");
+ return aos::configuration::GetNode(&configuration_.message(),
+ "roborio");
+ }()),
+ selector_event_loop_(
+ event_loop_factory_.MakeEventLoop("drivetrain", roborio_node_)),
+ target_selector_(selector_event_loop_.get()),
+ test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", roborio_node_)),
+ constants_fetcher_(test_event_loop_.get()),
+ joystick_state_sender_(
+ test_event_loop_->MakeSender<aos::JoystickState>("/aos")),
+ hint_sender_(
+ test_event_loop_->MakeSender<TargetSelectorHint>("/drivetrain")) {}
+
+ void SendJoystickState() {
+ auto builder = joystick_state_sender_.MakeBuilder();
+ aos::JoystickState::Builder state_builder =
+ builder.MakeBuilder<aos::JoystickState>();
+ state_builder.add_alliance(aos::Alliance::kRed);
+ builder.CheckOk(builder.Send(state_builder.Finish()));
+ }
+
+ void SendHint(GridSelectionHint grid, RowSelectionHint row,
+ SpotSelectionHint spot) {
+ auto builder = hint_sender_.MakeBuilder();
+ builder.CheckOk(builder.Send(
+ CreateTargetSelectorHint(*builder.fbb(), grid, row, spot)));
+ }
+ void SendHint(RowSelectionHint row, SpotSelectionHint spot) {
+ auto builder = hint_sender_.MakeBuilder();
+ TargetSelectorHint::Builder hint_builder =
+ builder.MakeBuilder<TargetSelectorHint>();
+ hint_builder.add_row(row);
+ hint_builder.add_spot(spot);
+ builder.CheckOk(builder.Send(hint_builder.Finish()));
+ }
+
+ const localizer::HalfField *scoring_map() const {
+ return constants_fetcher_.constants().scoring_map()->red();
+ }
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+ const aos::Node *const roborio_node_;
+ std::unique_ptr<aos::EventLoop> selector_event_loop_;
+ TargetSelector target_selector_;
+ std::unique_ptr<aos::EventLoop> test_event_loop_;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
+ aos::Sender<aos::JoystickState> joystick_state_sender_;
+ aos::Sender<TargetSelectorHint> hint_sender_;
+};
+
+// Tests that no target is available if no input messages have been sent.
+TEST_F(TargetSelectorTest, NoTargetWithoutInputs) {
+ EXPECT_FALSE(target_selector_.UpdateSelection(
+ Eigen::Matrix<double, 5, 1>::Zero(), 0.0));
+ EXPECT_DEATH(target_selector_.TargetPose(), "Did you check the return value");
+ EXPECT_EQ(0.0, target_selector_.TargetRadius());
+}
+
+// Tests that if we fully specify which target to go to that we always will do
+// so.
+TEST_F(TargetSelectorTest, FullySpecifiedTarget) {
+ SendJoystickState();
+ // Iterate over every available target.
+ for (const auto &[grid, grid_hint] : std::vector<
+ std::pair<const localizer::ScoringGrid *, GridSelectionHint>>{
+ {scoring_map()->left_grid(), GridSelectionHint::LEFT},
+ {scoring_map()->middle_grid(), GridSelectionHint::MIDDLE},
+ {scoring_map()->right_grid(), GridSelectionHint::RIGHT}}) {
+ for (const auto &[row, row_hint] : std::vector<
+ std::pair<const localizer::ScoringRow *, RowSelectionHint>>{
+ {grid->bottom(), RowSelectionHint::BOTTOM},
+ {grid->middle(), RowSelectionHint::MIDDLE},
+ {grid->top(), RowSelectionHint::TOP}}) {
+ for (const auto &[spot, spot_hint] : std::vector<
+ std::pair<const frc971::vision::Position *, SpotSelectionHint>>{
+ {row->left_cone(), SpotSelectionHint::LEFT},
+ {row->cube(), SpotSelectionHint::MIDDLE},
+ {row->right_cone(), SpotSelectionHint::RIGHT}}) {
+ SendHint(grid_hint, row_hint, spot_hint);
+ 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());
+ }
+ }
+ }
+}
+
+// Tests that if we leave the grid setting ambiguous that we select the
+// nearest possible target given the other settings.
+TEST_F(TargetSelectorTest, NoGridSpecified) {
+ SendJoystickState();
+ // We will leave the robot at (0, 0). This means that if we are going for the
+ // left cone we should go for the middle grid, and if we are going for the
+ // cube (middle) or right cone positions we should prefer the left grid.
+ // Note that the grids are not centered on the field (hence the middle isn't
+ // always preferred when at (0, 0)).
+
+ for (const auto &[spot, spot_hint] : std::vector<
+ std::pair<const frc971::vision::Position *, SpotSelectionHint>>{
+ {scoring_map()->middle_grid()->bottom()->left_cone(),
+ SpotSelectionHint::LEFT},
+ {scoring_map()->left_grid()->bottom()->cube(),
+ SpotSelectionHint::MIDDLE},
+ {scoring_map()->left_grid()->bottom()->right_cone(),
+ SpotSelectionHint::RIGHT}}) {
+ SendHint(RowSelectionHint::BOTTOM, spot_hint);
+ 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());
+ }
+}
+
+} // namespace y2023::control_loops::drivetrain