Make target selector use throttle instead of velocity
Change-Id: I7f3c1cc593854f957ffe7f9c29e6dd5769ab0f69
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index d085d17..b5417af 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -182,7 +182,8 @@
// Because we assume the target selector may have some internal state (e.g.,
// not confirming a target until some time as passed), we should call
// UpdateSelection every time.
- bool new_target = target_selector_->UpdateSelection(abs_state);
+ bool new_target =
+ target_selector_->UpdateSelection(abs_state, goal_velocity_);
if (freeze_target_) {
// When freezing the target, only make changes if we didn't have a good
// target before.
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index f0d81cb..f0685dd 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -16,9 +16,12 @@
// 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.
+ // command_speed is the goal speed of the current drivetrain, generally
+ // generated from the throttle and meant to signify driver intent.
// TODO(james): Some implementations may also want a drivetrain goal so that
// driver intent can be divined more directly.
- virtual bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state) = 0;
+ virtual bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
+ double command_speed) = 0;
// Gets the current target pose. Should only be called if UpdateSelection has
// returned true.
virtual TypedPose<double> TargetPose() const = 0;
@@ -59,7 +62,7 @@
// manually set the target selector state.
class TrivialTargetSelector : public TargetSelectorInterface {
public:
- bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &) override {
+ bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &, double) override {
return has_target_;
}
TypedPose<double> TargetPose() const override { return pose_; }
diff --git a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
index a3bf9a4..e086e36 100644
--- a/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
+++ b/y2019/control_loops/drivetrain/localized_drivetrain_test.cc
@@ -310,11 +310,11 @@
SetStartingPosition({4, 3, M_PI});
my_drivetrain_queue_.goal.MakeWithBuilder()
.controller_type(3)
- .throttle(0.5)
+ .throttle(0.9)
.Send();
RunForTime(chrono::seconds(10));
- VerifyEstimatorAccurate(1e-10);
+ VerifyEstimatorAccurate(1e-8);
// Due to the fact that we aren't modulating the throttle, we don't try to hit
// the target exactly. Instead, just run slightly past the target:
EXPECT_LT(::std::abs(::aos::math::DiffAngle(
diff --git a/y2019/control_loops/drivetrain/target_selector.cc b/y2019/control_loops/drivetrain/target_selector.cc
index 2b70ab7..449ce64 100644
--- a/y2019/control_loops/drivetrain/target_selector.cc
+++ b/y2019/control_loops/drivetrain/target_selector.cc
@@ -11,9 +11,9 @@
back_viewer_({&robot_pose_, {0.0, 0.0, 0.0}, M_PI}, kFakeFov, fake_noise_,
constants::Field().targets(), {}) {}
-bool TargetSelector::UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state) {
- const double speed = (state(3, 0) + state(4, 0)) / 2.0;
- if (::std::abs(speed) < kMinDecisionSpeed) {
+bool TargetSelector::UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
+ double command_speed) {
+ if (::std::abs(command_speed) < kMinDecisionSpeed) {
return false;
}
*robot_pose_.mutable_pos() << state.x(), state.y(), 0.0;
@@ -21,7 +21,7 @@
::aos::SizedArray<FakeCamera::TargetView,
y2019::constants::Field::kNumTargets>
target_views;
- if (speed > 0) {
+ if (command_speed > 0) {
target_views = front_viewer_.target_views();
} else {
target_views = back_viewer_.target_views();
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index d3f1a9d..965d7cc 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -29,14 +29,15 @@
TargetSelector();
- bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state) override;
+ bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
+ double command_speed) override;
Pose TargetPose() const override { return target_pose_; }
private:
- static constexpr double kFakeFov = M_PI;
+ static constexpr double kFakeFov = M_PI * 0.7;
// Longitudinal speed at which the robot must be going in order for us to make
// a decision.
- static constexpr double kMinDecisionSpeed = 0.1; // m/s
+ static constexpr double kMinDecisionSpeed = 0.7; // m/s
Pose robot_pose_;
Pose target_pose_;
// For the noise of our fake cameras, we only really care about the max
diff --git a/y2019/control_loops/drivetrain/target_selector_test.cc b/y2019/control_loops/drivetrain/target_selector_test.cc
index 7c0ad7f..4b440c2 100644
--- a/y2019/control_loops/drivetrain/target_selector_test.cc
+++ b/y2019/control_loops/drivetrain/target_selector_test.cc
@@ -18,10 +18,12 @@
// Tests the target selector with:
// -The [x, y, theta, left_vel, right_vel] state to test at
+// -The current driver commanded speed.
// -Whether we expect to see a target.
// -If (1) is true, the pose we expect to get back.
struct TestParams {
State state;
+ double command_speed;
bool expect_target;
Pose expected_pose;
};
@@ -31,7 +33,8 @@
TargetSelector selector;
bool expect_target = GetParam().expect_target;
const State state = GetParam().state;
- ASSERT_EQ(expect_target, selector.UpdateSelection(state))
+ ASSERT_EQ(expect_target,
+ selector.UpdateSelection(state, GetParam().command_speed))
<< "We expected a return of " << expect_target << " at state "
<< state.transpose();
if (expect_target) {
@@ -54,31 +57,42 @@
::testing::Values(
// When we are far away from anything, we should not register any
// targets:
- TestParams{(State() << 0.0, 0.0, 0.0, 1.0, 1.0).finished(), false, {}},
+ TestParams{
+ (State() << 0.0, 0.0, 0.0, 1.0, 1.0).finished(), 1.0, false, {}},
// Aim for a human-player spot; at low speeds we should not register
// anything.
- TestParams{
- (State() << 4.0, 2.0, M_PI, 0.05, 0.05).finished(), false, {}},
- TestParams{
- (State() << 4.0, 2.0, M_PI, -0.05, -0.05).finished(), false, {}},
- TestParams{(State() << 4.0, 2.0, M_PI, 0.5, 0.5).finished(), true,
+ TestParams{(State() << 4.0, 2.0, M_PI, 0.05, 0.05).finished(),
+ 0.05,
+ false,
+ {}},
+ TestParams{(State() << 4.0, 2.0, M_PI, -0.05, -0.05).finished(),
+ -0.05,
+ false,
+ {}},
+ TestParams{(State() << 4.0, 2.0, M_PI, 0.5, 0.5).finished(), 1.0, true,
HPSlotLeft()},
// Put ourselves between the rocket and cargo ship; we should see the
// portal driving one direction and the near cargo ship port the other.
- TestParams{(State() << 6.0, 2.0, -M_PI_2, 0.5, 0.5).finished(), true,
- CargoNearLeft()},
- TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, -0.5).finished(), true,
- CargoNearLeft()},
- TestParams{(State() << 6.0, 2.0, -M_PI_2, -0.5, -0.5).finished(), true,
- RocketPortalLeft()},
- TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(), true,
- RocketPortalLeft()},
+ // We also command a speed opposite the current direction of motion and
+ // confirm that that behaves as expected.
+ TestParams{(State() << 6.0, 2.0, -M_PI_2, -0.5, -0.5).finished(), 1.0,
+ true, CargoNearLeft()},
+ TestParams{(State() << 6.0, 2.0, M_PI_2, 0.5, 0.5).finished(), -1.0,
+ true, CargoNearLeft()},
+ TestParams{(State() << 6.0, 2.0, -M_PI_2, 0.5, 0.5).finished(), -1.0,
+ true, RocketPortalLeft()},
+ TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, -0.5).finished(), 1.0,
+ true, RocketPortalLeft()},
// And we shouldn't see anything spinning in place:
- TestParams{
- (State() << 6.0, 2.0, M_PI_2, -0.5, 0.5).finished(), false, {}},
+ TestParams{(State() << 6.0, 2.0, M_PI_2, -0.5, 0.5).finished(),
+ 0.0,
+ false,
+ {}},
// Drive backwards off the field--we should not see anything.
- TestParams{
- (State() << -0.1, 0.0, 0.0, -0.5, -0.5).finished(), false, {}}));
+ TestParams{(State() << -0.1, 0.0, 0.0, -0.5, -0.5).finished(),
+ -1.0,
+ false,
+ {}}));
} // namespace testing
} // namespace control_loops