Fix blue-side & substation auto-align
We weren't setting the angle of the targets on the far side of the
field.
Change-Id: If586b548b36323ff7b49d4a1823689a8edd7c04e
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 101c0f7..ea20bd0 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -165,6 +165,8 @@
const ::Eigen::Matrix<double, 5, 1> &abs_state, double relative_y_offset,
double velocity_sign) {
// Calculates the goal angle for the drivetrain given our position.
+ // Note: The goal angle is in target-relative coordinates, since our entire
+ // control loop is written relative to the target.
// The calculated goal will be such that a point piece_rad to one side of the
// drivetrain (the side depends on where we approach from and SignedRadii())
// will end up hitting the plane of the target exactly target_rad from the
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index 0fb5df5..bc1fb90 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -171,7 +171,10 @@
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);
+ // Since all targets on one side of the field face the same direction, we
+ // can just auto-choose orientation based on field side.
+ target_pose_ = Pose(closest_position.value(),
+ /*theta=*/closest_position->x() > 0.0 ? 0.0 : M_PI);
if (hint_fetcher_->has_robot_side()) {
drive_direction_ = hint_fetcher_->robot_side();
} else {
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index cce91c7..8d04056 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -528,6 +528,7 @@
auto builder = target_selector_hint_sender_.MakeBuilder();
auto hint_builder = builder.MakeBuilder<TargetSelectorHint>();
hint_builder.add_substation_pickup(true);
+ 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");
}