Reduce hysteresis in 2023 target selector
This should make things more reasonable for the drivers, such that the
manipulator changing the button forces us to realign.
Change-Id: I31a9d6527654706f96d9d0b57c35721599ce5336
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 124af4b..f231b47 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -209,7 +209,7 @@
// UpdateSelection every time.
bool new_target =
target_selector_->UpdateSelection(abs_state, goal_velocity_);
- if (freeze_target_) {
+ if (freeze_target_ && !target_selector_->ForceReselectTarget()) {
// When freezing the target, only make changes if we didn't have a good
// target before.
if (!have_target_ && new_target) {
@@ -229,9 +229,6 @@
goal_velocity_);
}
}
- // TODO(james): Find out how often the manipulator hits the right button first
- // try. We may want to make our target freezing logic more permissive,
- // especially for drive direction.
// Get the robot pose in the target coordinate frame.
relative_pose_ = Pose({abs_state.x(), abs_state.y(), 0.0}, abs_state(2, 0))
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index 672464f..3ea0b0b 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -260,6 +260,27 @@
<< "Expected state of zero, got: " << state_.transpose();
}
+// Tests that, when we explicitly require that the controller use a new target
+// that we obey it, even during trying to do line following.
+TEST_F(LineFollowDrivetrainTest, IgnoreFreezeWhenRequested) {
+ state_.setZero();
+ set_goal_pose({{0.0, 0.0, 0.0}, 0.0});
+ // Do one iteration to get the target into the drivetrain:
+ Iterate();
+
+ freeze_target_ = true;
+ target_selector_.set_force_reselect(true);
+
+ // Set a goal pose that we will end up trying to go to.
+ set_goal_pose({{1.0, 0.0, 0.0}, 0.0});
+ driver_model_ = [](const ::Eigen::Matrix<double, 5, 1> &) { return 0.25; };
+
+ RunForTime(::std::chrono::seconds(5));
+ // Should've driven a decent distance in X.
+ EXPECT_LT(1.0, state_.x());
+ EXPECT_NEAR(0.0, state_.y(), 1e-6);
+}
+
// Tests that when we freeze the controller without having acquired a target, we
// don't do anything until a target arrives.
TEST_F(LineFollowDrivetrainTest, FreezeWithoutAcquiringTarget) {
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index c805f35..6e53138 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -36,6 +36,9 @@
virtual double TargetRadius() const = 0;
// Which direction we want the robot to drive to get to the target.
virtual Side DriveDirection() const = 0;
+ // Indicates that the line following *must* drive to the currently selected
+ // target, regardless of any hysteresis we try to use to protect the driver.
+ virtual bool ForceReselectTarget() const = 0;
};
// Defines an interface for classes that provide field-global localization.
@@ -119,15 +122,20 @@
TypedPose<double> TargetPose() const override { return pose_; }
double TargetRadius() const override { return target_radius_; }
Side DriveDirection() const override { return drive_direction_; }
+ bool ForceReselectTarget() const override { return force_reselect_; }
void set_pose(const TypedPose<double> &pose) { pose_ = pose; }
void set_target_radius(double radius) { target_radius_ = radius; }
void set_has_target(bool has_target) { has_target_ = has_target; }
void set_drive_direction(Side side) { drive_direction_ = side; }
+ void set_force_reselect(bool force_reselect) {
+ force_reselect_ = force_reselect;
+ }
bool has_target() const { return has_target_; }
private:
bool has_target_ = true;
+ bool force_reselect_ = false;
TypedPose<double> pose_;
double target_radius_ = 0.0;
Side drive_direction_ = Side::DONT_CARE;
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index c7194f9..1fdd691 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -1,10 +1,10 @@
#ifndef Y2019_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
#define Y2019_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
-#include "frc971/control_loops/pose.h"
-#include "frc971/control_loops/drivetrain/localizer.h"
-#include "y2019/constants.h"
#include "frc971/control_loops/drivetrain/camera.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/pose.h"
+#include "y2019/constants.h"
#include "y2019/control_loops/drivetrain/target_selector_generated.h"
#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
@@ -29,7 +29,8 @@
// obstacles and just assume that we have perfect field of view.
typedef frc971::control_loops::TypedCamera<
y2019::constants::Field::kNumTargets,
- /*num_obstacles=*/0, double> FakeCamera;
+ /*num_obstacles=*/0, double>
+ FakeCamera;
TargetSelector(::aos::EventLoop *event_loop);
virtual ~TargetSelector() {}
@@ -42,6 +43,8 @@
Side DriveDirection() const override { return Side::DONT_CARE; }
+ bool ForceReselectTarget() const override { return false; }
+
private:
static constexpr double kFakeFov = M_PI * 0.9;
// Longitudinal speed at which the robot must be going in order for us to make
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index e49ba2f..20e5e25 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -3,6 +3,12 @@
#include "aos/containers/sized_array.h"
namespace y2023::control_loops::drivetrain {
+namespace {
+// If we already have a target selected, require the robot to be closer than
+// this distance (in meters) to one target than another before swapping.
+constexpr double kGridHysteresisDistance = 0.1;
+} // namespace
+
TargetSelector::TargetSelector(aos::EventLoop *event_loop)
: joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
@@ -44,6 +50,17 @@
// We don't know where to go, wait on a hint.
return false;
}
+ // Keep track of when the hint changes (note that this will not detect default
+ // vs. not populated default values); when it changes, force us to reselect
+ // the target.
+ {
+ TargetSelectorHintT hint_object;
+ hint_fetcher_.get()->UnPackTo(&hint_object);
+ if (!last_hint_.has_value() || hint_object != last_hint_) {
+ target_pose_.reset();
+ }
+ last_hint_ = hint_object;
+ }
aos::SizedArray<const localizer::ScoringGrid *, 3> possible_grids;
if (hint_fetcher_->has_grid()) {
possible_grids = {[this]() -> const localizer::ScoringGrid * {
@@ -102,24 +119,35 @@
return positions;
}();
CHECK_LT(0u, possible_positions.size());
+ aos::SizedArray<double, 3> distances;
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();
+ distances.push_back(distance);
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);
- if (hint_fetcher_->has_robot_side()) {
- drive_direction_ = hint_fetcher_->robot_side();
- } else {
- drive_direction_ = Side::DONT_CARE;
+ std::sort(distances.begin(), distances.end());
+ CHECK_EQ(distances.at(0), closest_distance.value());
+ // Only change the target pose if one grid is clearly better than the other.
+ // This prevents us from dithering between two grids if we happen to be on the
+ // boundary.
+ if (!target_pose_.has_value() ||
+ distances.at(1) - distances.at(0) > kGridHysteresisDistance) {
+ 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;
+ }
}
+ CHECK(target_pose_.has_value());
return true;
}
diff --git a/y2023/control_loops/drivetrain/target_selector.h b/y2023/control_loops/drivetrain/target_selector.h
index 9c4600d..5afc846 100644
--- a/y2023/control_loops/drivetrain/target_selector.h
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -38,12 +38,15 @@
double TargetRadius() const override { return 0.0; }
Side DriveDirection() const override { return drive_direction_; }
+ // We will manage any desired hysteresis in the target selection.
+ bool ForceReselectTarget() const override { return true; }
private:
void UpdateAlliance();
std::optional<Pose> target_pose_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<TargetSelectorHint> hint_fetcher_;
+ std::optional<TargetSelectorHintT> last_hint_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
const localizer::HalfField *scoring_map_ = nullptr;
Side drive_direction_ = Side::DONT_CARE;
diff --git a/y2023/control_loops/drivetrain/target_selector_test.cc b/y2023/control_loops/drivetrain/target_selector_test.cc
index fbe5e8d..c28c14d 100644
--- a/y2023/control_loops/drivetrain/target_selector_test.cc
+++ b/y2023/control_loops/drivetrain/target_selector_test.cc
@@ -108,6 +108,7 @@
EXPECT_EQ(frc971::control_loops::drivetrain::TargetSelectorInterface::
Side::FRONT,
target_selector_.DriveDirection());
+ EXPECT_TRUE(target_selector_.ForceReselectTarget());
}
}
}
@@ -134,6 +135,7 @@
SendHint(RowSelectionHint::BOTTOM, spot_hint, Side::BACK);
EXPECT_TRUE(target_selector_.UpdateSelection(
Eigen::Matrix<double, 5, 1>::Zero(), 0.0));
+ EXPECT_TRUE(target_selector_.ForceReselectTarget());
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());
@@ -143,4 +145,44 @@
}
}
+// Tests that if we are on the boundary of two grids that we do apply some
+// hysteresis.
+TEST_F(TargetSelectorTest, GridHysteresis) {
+ 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)).
+
+ const frc971::vision::Position *left_pos =
+ scoring_map()->left_grid()->bottom()->cube();
+ const frc971::vision::Position *middle_pos =
+ scoring_map()->middle_grid()->bottom()->cube();
+ Eigen::Matrix<double, 5, 1> split_position;
+ split_position << 0.0, (left_pos->y() + middle_pos->y()) / 2.0, 0.0, 0.0, 0.0;
+ Eigen::Matrix<double, 5, 1> slightly_left = split_position;
+ slightly_left.y() += 0.01;
+ Eigen::Matrix<double, 5, 1> slightly_middle = split_position;
+ slightly_middle.y() -= 0.01;
+ Eigen::Matrix<double, 5, 1> very_middle = split_position;
+ very_middle.y() -= 1.0;
+
+ SendHint(RowSelectionHint::BOTTOM, SpotSelectionHint::MIDDLE, Side::BACK);
+ EXPECT_TRUE(target_selector_.UpdateSelection(slightly_left, 0.0));
+ Eigen::Vector3d target = target_selector_.TargetPose().abs_pos();
+ EXPECT_EQ(target.x(), left_pos->x());
+ EXPECT_EQ(target.y(), left_pos->y());
+ // A slight movement should *not* reset things.
+ EXPECT_TRUE(target_selector_.UpdateSelection(slightly_middle, 0.0));
+ target = target_selector_.TargetPose().abs_pos();
+ EXPECT_EQ(target.x(), left_pos->x());
+ EXPECT_EQ(target.y(), left_pos->y());
+ // A large movement *should* reset things.
+ EXPECT_TRUE(target_selector_.UpdateSelection(very_middle, 0.0));
+ target = target_selector_.TargetPose().abs_pos();
+ EXPECT_EQ(target.x(), middle_pos->x());
+ EXPECT_EQ(target.y(), middle_pos->y());
+}
+
} // namespace y2023::control_loops::drivetrain