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/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index f886bb2..89dc3d5 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -82,6 +82,12 @@
available_splines:[int] (id: 12);
}
+enum RobotSide : ubyte {
+ FRONT = 0,
+ BACK = 1,
+ DONT_CARE = 2,
+}
+
// For logging state of the line follower.
table LineFollowLogging {
// Whether we are currently freezing target choice.
@@ -100,6 +106,8 @@
goal_theta:float (id: 7);
// Current relative heading.
rel_theta:float (id: 8);
+ // Current goal drive direction.
+ drive_direction:RobotSide = DONT_CARE (id: 9);
}
// Current states of the EKF. See hybrid_ekf.h for detailed comments.
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 95f14a3..124af4b 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -87,6 +87,19 @@
return Kff;
}
+double VelocitySignForSide(TargetSelectorInterface::Side side,
+ double goal_velocity) {
+ switch (side) {
+ case TargetSelectorInterface::Side::FRONT:
+ return 1.0;
+ case TargetSelectorInterface::Side::BACK:
+ return -1.0;
+ case TargetSelectorInterface::Side::DONT_CARE:
+ return goal_velocity >= 0.0 ? 1.0 : -1.0;
+ }
+ return 1.0;
+}
+
} // namespace
// When we create A/B, we do recompute A/B, but we don't really care about
@@ -202,7 +215,8 @@
if (!have_target_ && new_target) {
have_target_ = true;
start_of_target_acquire_ = now;
- velocity_sign_ = goal_velocity_ >= 0.0 ? 1.0 : -1.0;
+ velocity_sign_ = VelocitySignForSide(target_selector_->DriveDirection(),
+ goal_velocity_);
target_pose_ = target_selector_->TargetPose();
}
} else {
@@ -211,9 +225,14 @@
have_target_ = new_target;
if (have_target_) {
target_pose_ = target_selector_->TargetPose();
- velocity_sign_ = goal_velocity_ >= 0.0 ? 1.0 : -1.0;
+ velocity_sign_ = VelocitySignForSide(target_selector_->DriveDirection(),
+ 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))
.Rebase(&target_pose_);
@@ -276,6 +295,8 @@
-relative_pose_.rel_pos().x());
line_follow_logging_builder.add_goal_theta(controls_goal_(0, 0));
line_follow_logging_builder.add_rel_theta(relative_pose_.rel_theta());
+ line_follow_logging_builder.add_drive_direction(
+ target_selector_->DriveDirection());
return line_follow_logging_builder.Finish();
}
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index bb872dc..672464f 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -309,12 +309,12 @@
VerifyNearGoal();
}
INSTANTIATE_TEST_SUITE_P(TargetPosTest, LineFollowDrivetrainTargetParamTest,
- ::testing::Values(Pose({0.0, 0.0, 0.0}, 0.0),
- Pose({1.0, 0.0, 0.0}, 0.0),
- Pose({3.0, 1.0, 0.0}, 0.0),
- Pose({3.0, 0.0, 0.0}, 0.5),
- Pose({3.0, 0.0, 0.0}, -0.5),
- Pose({-3.0, -1.0, 0.0}, -2.5)));
+ ::testing::Values(Pose({0.0, 0.0, 0.0}, 0.0),
+ Pose({1.0, 0.0, 0.0}, 0.0),
+ Pose({3.0, 1.0, 0.0}, 0.0),
+ Pose({3.0, 0.0, 0.0}, 0.5),
+ Pose({3.0, 0.0, 0.0}, -0.5),
+ Pose({-3.0, -1.0, 0.0}, -2.5)));
class LineFollowDrivetrainParamTest
: public LineFollowDrivetrainTest,
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index d590f1d..c805f35 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:
+ typedef RobotSide Side;
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
@@ -33,6 +34,8 @@
// from the TargetPose--this is distinct from wanting the center of the
// robot to project straight onto the center of the target.
virtual double TargetRadius() const = 0;
+ // Which direction we want the robot to drive to get to the target.
+ virtual Side DriveDirection() const = 0;
};
// Defines an interface for classes that provide field-global localization.
@@ -115,16 +118,19 @@
}
TypedPose<double> TargetPose() const override { return pose_; }
double TargetRadius() const override { return target_radius_; }
+ Side DriveDirection() const override { return drive_direction_; }
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; }
bool has_target() const { return has_target_; }
private:
bool has_target_ = true;
TypedPose<double> pose_;
double target_radius_ = 0.0;
+ Side drive_direction_ = Side::DONT_CARE;
};
// Uses the generic HybridEkf implementation to provide a basic field estimator.
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index 19ac52e..c7194f9 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -40,6 +40,8 @@
double TargetRadius() const override { return target_radius_; }
+ Side DriveDirection() const override { return Side::DONT_CARE; }
+
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/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());
}
}
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 135c987..43962dd 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -31,11 +31,12 @@
using frc971::input::driver_station::ControlBit;
using frc971::input::driver_station::JoystickAxis;
using frc971::input::driver_station::POVLocation;
-using y2023::control_loops::superstructure::RollerGoal;
-using y2023::control_loops::drivetrain::RowSelectionHint;
using y2023::control_loops::drivetrain::GridSelectionHint;
+using y2023::control_loops::drivetrain::RowSelectionHint;
using y2023::control_loops::drivetrain::SpotSelectionHint;
using y2023::control_loops::drivetrain::TargetSelectorHint;
+using y2023::control_loops::superstructure::RollerGoal;
+using Side = frc971::control_loops::drivetrain::RobotSide;
namespace y2023 {
namespace input {
@@ -77,11 +78,6 @@
CUBE = 2,
};
-enum class Side {
- FRONT = 0,
- BACK = 1,
-};
-
struct ButtonData {
ButtonLocation button;
std::optional<SpotSelectionHint> spot = std::nullopt;
@@ -439,8 +435,7 @@
auto hint_builder = builder.MakeBuilder<TargetSelectorHint>();
hint_builder.add_row(placing_row.value());
hint_builder.add_spot(placing_spot.value());
- // TODO: Add field to TargetSelector hint for forwards vs. backwards
- // placement.
+ hint_builder.add_robot_side(CHECK_NOTNULL(current_setpoint_)->side);
if (builder.Send(hint_builder.Finish()) != aos::RawSender::Error::kOk) {
AOS_LOG(ERROR, "Sending target selector hint failed.\n");
}