Add tof sensor calibration
Change-Id: I4bc2107bbf76f0c5f98ff749ada673c656e1c1ed
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index a12326c..41d6542 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -125,6 +125,15 @@
)
flatbuffer_cc_library(
+ name = "target_selector_status_fbs",
+ srcs = [
+ ":target_selector_status.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_cc_library(
name = "target_selector_hint_fbs",
srcs = [
":target_selector_hint.fbs",
@@ -142,13 +151,16 @@
hdrs = ["target_selector.h"],
deps = [
":target_selector_hint_fbs",
+ ":target_selector_status_fbs",
"//aos/containers:sized_array",
"//aos/events:event_loop",
"//frc971/constants:constants_sender_lib",
"//frc971/control_loops:pose",
"//frc971/control_loops/drivetrain:localizer",
"//frc971/input:joystick_state_fbs",
+ "//frc971/shooter_interpolation:interpolation",
"//y2023/constants:constants_fbs",
+ "//y2023/control_loops/superstructure:superstructure_position_fbs",
],
)
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index 20e5e25..26f378c 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -1,6 +1,8 @@
#include "y2023/control_loops/drivetrain/target_selector.h"
#include "aos/containers/sized_array.h"
+#include "frc971/shooter_interpolation/interpolation.h"
+#include "y2023/control_loops/superstructure/superstructure_position_generated.h"
namespace y2023::control_loops::drivetrain {
namespace {
@@ -13,10 +15,25 @@
: joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
hint_fetcher_(event_loop->MakeFetcher<TargetSelectorHint>("/drivetrain")),
+ status_sender_(
+ event_loop->MakeSender<TargetSelectorStatus>("/drivetrain")),
constants_fetcher_(event_loop) {
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) {
+ game_piece_position_ =
+ LateralOffsetForTimeOfFlight(msg.cone_position());
+ });
+
+ event_loop->AddPhasedLoop([this](int){
+ auto builder = status_sender_.MakeBuilder();
+ auto status_builder = builder.MakeBuilder<TargetSelectorStatus>();
+ status_builder.add_game_piece_position(game_piece_position_);
+ builder.CheckOk(builder.Send(status_builder.Finish()));
+ }, std::chrono::milliseconds(100));
}
void TargetSelector::UpdateAlliance() {
@@ -151,4 +168,21 @@
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) const {
+ 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 b1a9a39..cab4816 100644
--- a/y2023/control_loops/drivetrain/target_selector.h
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -6,6 +6,7 @@
#include "frc971/input/joystick_state_generated.h"
#include "y2023/constants/constants_generated.h"
#include "y2023/control_loops/drivetrain/target_selector_hint_generated.h"
+#include "y2023/control_loops/drivetrain/target_selector_status_generated.h"
namespace y2023::control_loops::drivetrain {
// This target selector provides the logic to choose which position to try to
@@ -37,7 +38,7 @@
}
double TargetRadius() const override { return 0.0; }
- double GamePieceRadius() const override { return 0.0; }
+ double GamePieceRadius() const override { return game_piece_position_; }
bool SignedRadii() const override { return true; }
Side DriveDirection() const override { return drive_direction_; }
// We will manage any desired hysteresis in the target selection.
@@ -45,12 +46,16 @@
private:
void UpdateAlliance();
+ // Returns the Y coordinate of a game piece given the time-of-flight reading.
+ double LateralOffsetForTimeOfFlight(double reading) const;
std::optional<Pose> target_pose_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<TargetSelectorHint> hint_fetcher_;
+ aos::Sender<TargetSelectorStatus> status_sender_;
std::optional<TargetSelectorHintT> last_hint_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
const localizer::HalfField *scoring_map_ = nullptr;
+ double game_piece_position_ = 0.0;
Side drive_direction_ = Side::DONT_CARE;
};
} // namespace y2023::control_loops::drivetrain
diff --git a/y2023/control_loops/drivetrain/target_selector_status.fbs b/y2023/control_loops/drivetrain/target_selector_status.fbs
new file mode 100644
index 0000000..2ca0a91
--- /dev/null
+++ b/y2023/control_loops/drivetrain/target_selector_status.fbs
@@ -0,0 +1,7 @@
+namespace y2023.control_loops.drivetrain;
+
+table TargetSelectorStatus {
+ game_piece_position:double (id: 0);
+}
+
+root_type TargetSelectorStatus;
diff --git a/y2023/control_loops/superstructure/BUILD b/y2023/control_loops/superstructure/BUILD
index e6f14aa..a4cb337 100644
--- a/y2023/control_loops/superstructure/BUILD
+++ b/y2023/control_loops/superstructure/BUILD
@@ -10,9 +10,9 @@
"superstructure_goal.fbs",
],
gen_reflections = 1,
- includes = [
- "//frc971/control_loops:control_loops_fbs_includes",
- "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
],
)
@@ -30,9 +30,9 @@
"superstructure_status.fbs",
],
gen_reflections = 1,
- includes = [
- "//frc971/control_loops:control_loops_fbs_includes",
- "//frc971/control_loops:profiled_subsystem_fbs_includes",
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
],
)
@@ -53,11 +53,11 @@
"superstructure_position.fbs",
],
gen_reflections = 1,
- includes = [
- "//frc971/control_loops:control_loops_fbs_includes",
- "//frc971/control_loops:profiled_subsystem_fbs_includes",
- "//frc971/vision:calibration_fbs_includes",
- "//y2023/control_loops/drivetrain:drivetrain_can_position_fbs_includes",
+ deps = [
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops:profiled_subsystem_fbs",
+ "//frc971/vision:calibration_fbs",
+ "//y2023/control_loops/drivetrain:drivetrain_can_position_fbs",
],
)