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;