Refactor game piece position code & latch in target selector
Move the conversions to where they belong in the superstructure code,
and make the target selector latch the game piece position while it
has a target.
Change-Id: I4cd96fad9f327a241a146024ba38bcf34dfc8564
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index 16a0090..1b70ca1 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -24,16 +24,6 @@
CHECK(constants_fetcher_.constants().has_scoring_map());
CHECK(constants_fetcher_.constants().scoring_map()->has_red());
CHECK(constants_fetcher_.constants().scoring_map()->has_blue());
- event_loop->MakeWatcher(
- "/superstructure",
- [this](const y2023::control_loops::superstructure::Position &msg) {
- // Technically this means that even if we have a cube we are relying on
- // getting a Position message before updating the game_piece_position_
- // to zero. But if we aren't getting position messages, then things are
- // very broken.
- game_piece_position_ =
- LateralOffsetForTimeOfFlight(msg.cone_position());
- });
event_loop->AddPhasedLoop(
[this](int) {
@@ -172,42 +162,15 @@
} else {
drive_direction_ = Side::DONT_CARE;
}
+ // Only update the game piece position when we reassign the target.
+ superstructure_status_fetcher_.Fetch();
+ if (superstructure_status_fetcher_.get() != nullptr) {
+ game_piece_position_ =
+ superstructure_status_fetcher_->game_piece_position();
+ }
}
CHECK(target_pose_.has_value());
return true;
}
-// TODO: Maybe this already handles field side correctly? Unsure if the line
-// follower ends up having positive as being robot frame relative or robot
-// direction relative...
-double TargetSelector::LateralOffsetForTimeOfFlight(double reading) {
- superstructure_status_fetcher_.Fetch();
- if (superstructure_status_fetcher_.get() != nullptr) {
- switch (superstructure_status_fetcher_->game_piece()) {
- case vision::Class::NONE:
- case vision::Class::CUBE:
- return 0.0;
- case vision::Class::CONE_UP:
- // execute logic below.
- break;
- case vision::Class::CONE_DOWN:
- // execute logic below.
- break;
- }
- } else {
- return 0.0;
- }
- const TimeOfFlight *calibration =
- CHECK_NOTNULL(constants_fetcher_.constants().robot()->tof());
- // TODO(james): Use a generic interpolation table class.
- auto table = CHECK_NOTNULL(calibration->interpolation_table());
- CHECK_EQ(2u, table->size());
- double x1 = table->Get(0)->tof_reading();
- double x2 = table->Get(1)->tof_reading();
- double y1 = table->Get(0)->lateral_position();
- double y2 = table->Get(1)->lateral_position();
- return frc971::shooter_interpolation::Blend((reading - x1) / (x2 - x1), y1,
- y2);
-}
-
} // namespace y2023::control_loops::drivetrain
diff --git a/y2023/control_loops/drivetrain/target_selector.h b/y2023/control_loops/drivetrain/target_selector.h
index bed56ce..5e7f015 100644
--- a/y2023/control_loops/drivetrain/target_selector.h
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -47,8 +47,6 @@
private:
void UpdateAlliance();
- // Returns the Y coordinate of a game piece given the time-of-flight reading.
- double LateralOffsetForTimeOfFlight(double reading);
std::optional<Pose> target_pose_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<TargetSelectorHint> hint_fetcher_;
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index 2700bbc..ad962ad 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -101,9 +101,13 @@
":superstructure_status_fbs",
"//aos:flatbuffer_merge",
"//aos/events:event_loop",
+ "//frc971/constants:constants_sender_lib",
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/shooter_interpolation:interpolation",
"//y2023:constants",
+ "//y2023/constants:constants_fbs",
+ "//y2023/constants:simulated_constants_sender",
"//y2023/control_loops/drivetrain:drivetrain_can_position_fbs",
"//y2023/control_loops/superstructure/arm",
"//y2023/control_loops/superstructure/arm:arm_trajectories_fbs",
diff --git a/y2023/control_loops/superstructure/superstructure.cc b/y2023/control_loops/superstructure/superstructure.cc
index 9b9b119..0f19a1e 100644
--- a/y2023/control_loops/superstructure/superstructure.cc
+++ b/y2023/control_loops/superstructure/superstructure.cc
@@ -3,6 +3,7 @@
#include "aos/events/event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "aos/network/team_number.h"
+#include "frc971/shooter_interpolation/interpolation.h"
#include "frc971/zeroing/wrap.h"
#include "y2023/control_loops/superstructure/arm/arm_trajectories_generated.h"
@@ -27,6 +28,7 @@
: frc971::controls::ControlLoop<Goal, Position, Status, Output>(event_loop,
name),
values_(values),
+ constants_fetcher_(event_loop),
drivetrain_status_fetcher_(
event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
@@ -100,6 +102,11 @@
status_builder.add_end_effector_state(end_effector_.state());
// TODO(milind): integrate this with ML game piece detection somehow
status_builder.add_game_piece(end_effector_.game_piece());
+ const std::optional<double> game_piece_position =
+ LateralOffsetForTimeOfFlight(position->cone_position());
+ if (game_piece_position.has_value()) {
+ status_builder.add_game_piece_position(game_piece_position.value());
+ }
(void)status->Send(status_builder.Finish());
}
@@ -110,6 +117,36 @@
: 0.0);
}
+std::optional<double> Superstructure::LateralOffsetForTimeOfFlight(
+ double reading) {
+ switch (end_effector_.game_piece()) {
+ case vision::Class::NONE:
+ return std::nullopt;
+ case vision::Class::CUBE:
+ // Cubes are definitionally centered.
+ return 0.0;
+ case vision::Class::CONE_UP:
+ case vision::Class::CONE_DOWN:
+ // execute logic below.
+ break;
+ }
+ constexpr double kInvalidReading = 0.93;
+ if (reading > kInvalidReading) {
+ return std::nullopt;
+ }
+ const TimeOfFlight *calibration = CHECK_NOTNULL(
+ CHECK_NOTNULL(constants_fetcher_.constants().robot())->tof());
+ // TODO(james): Use a generic interpolation table class.
+ auto table = CHECK_NOTNULL(calibration->interpolation_table());
+ CHECK_EQ(2u, table->size());
+ double x1 = table->Get(0)->tof_reading();
+ double x2 = table->Get(1)->tof_reading();
+ double y1 = table->Get(0)->lateral_position();
+ double y2 = table->Get(1)->lateral_position();
+ return frc971::shooter_interpolation::Blend((reading - x1) / (x2 - x1), y1,
+ y2);
+}
+
} // namespace superstructure
} // namespace control_loops
} // namespace y2023
diff --git a/y2023/control_loops/superstructure/superstructure.h b/y2023/control_loops/superstructure/superstructure.h
index 0c8b2c0..1100b86 100644
--- a/y2023/control_loops/superstructure/superstructure.h
+++ b/y2023/control_loops/superstructure/superstructure.h
@@ -3,9 +3,11 @@
#include "aos/events/event_loop.h"
#include "aos/json_to_flatbuffer.h"
+#include "frc971/constants/constants_sender_lib.h"
#include "frc971/control_loops/control_loop.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "y2023/constants.h"
+#include "y2023/constants/constants_generated.h"
#include "y2023/control_loops/drivetrain/drivetrain_can_position_generated.h"
#include "y2023/control_loops/superstructure/arm/arm.h"
#include "y2023/control_loops/superstructure/arm/arm_trajectories_generated.h"
@@ -63,7 +65,11 @@
aos::Sender<Status>::Builder *status) override;
private:
+ // Returns the Y coordinate of a game piece given the time-of-flight reading.
+ std::optional<double> LateralOffsetForTimeOfFlight(double reading);
+
std::shared_ptr<const constants::Values> values_;
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
aos::Fetcher<frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index e2524c1..c1c20a3 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -8,6 +8,7 @@
#include "frc971/control_loops/subsystem_simulator.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
+#include "y2023/constants/simulated_constants_sender.h"
#include "y2023/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
#include "y2023/control_loops/superstructure/superstructure.h"
@@ -240,8 +241,7 @@
position_builder.add_wrist(wrist_offset);
position_builder.add_end_effector_cube_beam_break(
end_effector_cube_beam_break_);
- // TODO(milind): put into our state
- position_builder.add_cone_position(0.95);
+ position_builder.add_cone_position(cone_position_);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
@@ -250,6 +250,10 @@
end_effector_cube_beam_break_ = triggered;
}
+ void set_cone_position(double cone_position) {
+ cone_position_ = cone_position;
+ }
+
private:
::aos::EventLoop *event_loop_;
const chrono::nanoseconds dt_;
@@ -265,6 +269,7 @@
::aos::Fetcher<Output> superstructure_output_fetcher_;
bool first_ = true;
+ double cone_position_ = 0.95;
};
class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
@@ -274,6 +279,8 @@
aos::configuration::ReadConfig("y2023/aos_config.json"),
std::chrono::microseconds(5050)),
values_(std::make_shared<constants::Values>(constants::MakeValues())),
+ simulated_constants_dummy_(SendSimulationConstants(
+ event_loop_factory(), 7971, "y2023/constants/test_constants.json")),
roborio_(aos::configuration::GetNode(configuration(), "roborio")),
logger_pi_(aos::configuration::GetNode(configuration(), "logger")),
arm_trajectories_(superstructure::Superstructure::GetArmTrajectories(
@@ -410,6 +417,7 @@
}
std::shared_ptr<const constants::Values> values_;
+ const bool simulated_constants_dummy_;
const aos::Node *const roborio_;
const aos::Node *const logger_pi_;
@@ -560,6 +568,42 @@
CheckIfZeroed();
}
+// Tests that the cone position ocnversion works.
+TEST_F(SuperstructureTest, ConePositionConversion) {
+ // Get ourselves into CONE mode.
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_arm_goal_position(arm::NeutralIndex());
+ goal_builder.add_trajectory_override(false);
+ goal_builder.add_roller_goal(RollerGoal::INTAKE_CONE_UP);
+ builder.CheckOk(builder.Send(goal_builder.Finish()));
+ }
+ superstructure_plant_.set_cone_position(1.0);
+ RunFor(chrono::seconds(1));
+ // Game piece position should not be populated when invalid.
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_FALSE(superstructure_status_fetcher_->has_game_piece_position());
+
+ // And then send a valid cone position.
+ superstructure_plant_.set_cone_position(0.5);
+ RunFor(chrono::seconds(1));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_TRUE(superstructure_status_fetcher_->has_game_piece_position());
+ EXPECT_FLOAT_EQ(0.0, superstructure_status_fetcher_->game_piece_position());
+
+ superstructure_plant_.set_cone_position(0.1);
+ RunFor(chrono::seconds(1));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(vision::Class::CONE_UP, superstructure_status_fetcher_->game_piece());
+ EXPECT_TRUE(superstructure_status_fetcher_->has_game_piece_position());
+ EXPECT_FLOAT_EQ(0.2, superstructure_status_fetcher_->game_piece_position());
+}
+
class SuperstructureBeambreakTest
: public SuperstructureTest,
public ::testing::WithParamInterface<vision::Class> {
diff --git a/y2023/control_loops/superstructure/superstructure_position.fbs b/y2023/control_loops/superstructure/superstructure_position.fbs
index 3c7a33b..83ca2b6 100644
--- a/y2023/control_loops/superstructure/superstructure_position.fbs
+++ b/y2023/control_loops/superstructure/superstructure_position.fbs
@@ -47,7 +47,13 @@
// Positive position would be upwards
wrist:frc971.AbsolutePosition (id: 1);
- // If this is true, the cone beam break is triggered.
+ // Estimated position of a cone in the gripper from the time-of-flight
+ // sensors.
+ // If greater than 0.9, indicates that we cannot see a cone.
+ // Will be larger when the cone is farther forwards on the robot when
+ // the wrist and arm positions are all at zero (this will typically mean
+ // that it is larger when the cone is to the robot's current right when
+ // trying to core).
cone_position:double (id: 2);
// If this is true, the cube beam break is triggered.
diff --git a/y2023/control_loops/superstructure/superstructure_status.fbs b/y2023/control_loops/superstructure/superstructure_status.fbs
index 5381b0a..8d74bfe 100644
--- a/y2023/control_loops/superstructure/superstructure_status.fbs
+++ b/y2023/control_loops/superstructure/superstructure_status.fbs
@@ -89,7 +89,15 @@
wrist:frc971.control_loops.AbsoluteEncoderProfiledJointStatus (id: 3);
end_effector_state:EndEffectorState (id: 4);
+
game_piece:vision.Class (id: 5);
+
+ // Indicates the current lateral position of the game piece, in meters.
+ // This number will be zero when the game piece is centered, and positive if
+ // the arm + wrist are all at zero and the game piece is towards the back
+ // of the robot. This will typically mean that positive is to the robot's
+ // left
+ game_piece_position:double (id: 6);
}
root_type Status;