Add tof sensor calibration
Change-Id: I4bc2107bbf76f0c5f98ff749ada673c656e1c1ed
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2023/BUILD b/y2023/BUILD
index 8108a39..a6fec71 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -204,6 +204,7 @@
"//y2019/control_loops/drivetrain:target_selector_fbs",
"//y2023/control_loops/superstructure:superstructure_goal_fbs",
"//y2023/control_loops/drivetrain:target_selector_hint_fbs",
+ "//y2023/control_loops/drivetrain:target_selector_status_fbs",
"//y2023/control_loops/drivetrain:drivetrain_can_position_fbs",
"//y2023/control_loops/superstructure:superstructure_output_fbs",
"//y2023/control_loops/superstructure:superstructure_position_fbs",
diff --git a/y2023/constants/7971.json b/y2023/constants/7971.json
index 7cbb37e..a3bd130 100644
--- a/y2023/constants/7971.json
+++ b/y2023/constants/7971.json
@@ -13,6 +13,5 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-7971-4_cam-23-04_ext_2023-02-19.json' %}
}
],
- "target_map": {% include 'y2023/vision/maps/target_map.json' %},
- "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
+ {% include 'y2023/constants/common.json' %}
}
diff --git a/y2023/constants/971.json b/y2023/constants/971.json
index 7e979c7..16681fb 100644
--- a/y2023/constants/971.json
+++ b/y2023/constants/971.json
@@ -13,6 +13,19 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-971-4_cam-23-08_ext_2023-02-22.json' %}
}
],
- "target_map": {% include 'y2023/vision/maps/target_map.json' %},
- "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
+ "robot": {
+ "tof": {
+ "interpolation_table": [
+ {
+ "tof_reading": 0.05,
+ "lateral_position": 0.23
+ },
+ {
+ "tof_reading": 0.90,
+ "lateral_position": -0.23
+ }
+ ]
+ }
+ },
+ {% include 'y2023/constants/common.json' %}
}
diff --git a/y2023/constants/9971.json b/y2023/constants/9971.json
index 356b232..a4eaff4 100644
--- a/y2023/constants/9971.json
+++ b/y2023/constants/9971.json
@@ -13,6 +13,19 @@
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-03-05.json' %}
}
],
- "target_map": {% include 'y2023/vision/maps/target_map.json' %},
- "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
+ "robot": {
+ "tof": {
+ "interpolation_table": [
+ {
+ "tof_reading": 0.05,
+ "lateral_position": 0.23
+ },
+ {
+ "tof_reading": 0.90,
+ "lateral_position": -0.23
+ }
+ ]
+ }
+ },
+ {% include 'y2023/constants/common.json' %}
}
diff --git a/y2023/constants/BUILD b/y2023/constants/BUILD
index 95d04a8..c91aa56 100644
--- a/y2023/constants/BUILD
+++ b/y2023/constants/BUILD
@@ -31,6 +31,7 @@
"7971.json",
"971.json",
"9971.json",
+ "common.json",
":scoring_map",
"//y2023/vision/calib_files",
"//y2023/vision/maps",
diff --git a/y2023/constants/common.json b/y2023/constants/common.json
new file mode 100644
index 0000000..c559810
--- /dev/null
+++ b/y2023/constants/common.json
@@ -0,0 +1,2 @@
+ "target_map": {% include 'y2023/vision/maps/target_map.json' %},
+ "scoring_map": {% include 'y2023/constants/scoring_map.json' %}
diff --git a/y2023/constants/constants.fbs b/y2023/constants/constants.fbs
index e874275..bd038b8 100644
--- a/y2023/constants/constants.fbs
+++ b/y2023/constants/constants.fbs
@@ -8,10 +8,32 @@
calibration:frc971.vision.calibration.CameraCalibration (id: 0);
}
+// Data point for a single time of flight sensor reading. Used in a linear
+// interpolation table.
+table TimeOfFlightDatum {
+ // Time-of-flight sensor reading for the datum.
+ tof_reading:double (id: 0);
+ // Where the game piece is laterally in the robot frame. 0 = centered;
+ // positive = to the left of the robot.
+ // In meters.
+ lateral_position:double (id: 1);
+}
+
+table TimeOfFlight {
+ interpolation_table:[TimeOfFlightDatum] (id: 0);
+}
+
+table RobotConstants {
+ // Table of time-of-flight reading positions. Until we bother using one
+ // of our interpolation classes, should just contain two values.
+ tof:TimeOfFlight (id: 0);
+}
+
table Constants {
cameras:[CameraConfiguration] (id: 0);
target_map:frc971.vision.TargetMap (id: 1);
scoring_map:localizer.ScoringMap (id: 2);
+ robot:RobotConstants (id: 3);
}
root_type Constants;
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",
],
)
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index 1c0beac..03985c0 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -412,7 +412,7 @@
},
{
"name": "/drivetrain",
- "type": "y2019.control_loops.drivetrain.TargetSelectorHint",
+ "type": "y2023.control_loops.drivetrain.TargetSelectorStatus",
"source_node": "roborio"
},
{