Swap to combined voltage/encoder message for localizer
Change-Id: I23b553b142bde46c321409105d49a716eb7059b6
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index c00ce90..789de2b 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -30,6 +30,11 @@
)
static_flatbuffer(
+ name = "rio_localizer_inputs_fbs",
+ srcs = ["rio_localizer_inputs.fbs"],
+)
+
+static_flatbuffer(
name = "drivetrain_output_fbs",
srcs = ["drivetrain_output.fbs"],
)
@@ -455,6 +460,7 @@
":localizer",
":localizer_fbs",
":polydrivetrain",
+ ":rio_localizer_inputs_fbs",
":spline_goal_fbs",
":splinedrivetrain",
":ssdrivetrain",
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2b84a9d..95fab0a2 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -350,7 +350,11 @@
dt_openloop_(dt_config_, filters_.kf()),
dt_closedloop_(dt_config_, filters_.kf(), localizer),
dt_spline_(dt_config_),
- dt_line_follow_(dt_config_, localizer->target_selector()) {
+ dt_line_follow_(dt_config_, localizer->target_selector()),
+ localizer_input_sender_(
+ event_loop->TryMakeSender<
+ frc971::control_loops::drivetrain::RioLocalizerInputsStatic>(
+ "/drivetrain")) {
event_loop->SetRuntimeRealtimePriority(30);
for (size_t ii = 0; ii < trajectory_fetchers_.size(); ++ii) {
trajectory_fetchers_[ii].fetcher =
@@ -613,6 +617,17 @@
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
+
+ if (localizer_input_sender_.valid()) {
+ auto localizer_input_builder = localizer_input_sender_.MakeStaticBuilder();
+ localizer_input_builder->set_left_encoder(position->left_encoder());
+ localizer_input_builder->set_right_encoder(position->right_encoder());
+ if (output) {
+ localizer_input_builder->set_left_voltage(output_struct.left_voltage);
+ localizer_input_builder->set_right_voltage(output_struct.right_voltage);
+ }
+ localizer_input_builder.CheckOk(localizer_input_builder.Send());
+ }
}
flatbuffers::Offset<Output> DrivetrainLoop::Zero(
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 87266f1..332b117 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -18,6 +18,7 @@
#include "frc971/control_loops/drivetrain/localizer.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/control_loops/drivetrain/polydrivetrain.h"
+#include "frc971/control_loops/drivetrain/rio_localizer_inputs_static.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/control_loops/polytope.h"
@@ -191,6 +192,8 @@
bool has_been_enabled_ = false;
aos::SendFailureCounter status_failure_counter_;
+ aos::Sender<::frc971::control_loops::drivetrain::RioLocalizerInputsStatic>
+ localizer_input_sender_;
};
} // namespace frc971::control_loops::drivetrain
diff --git a/frc971/control_loops/drivetrain/localization/BUILD b/frc971/control_loops/drivetrain/localization/BUILD
index 747b16c..1cf83d4 100644
--- a/frc971/control_loops/drivetrain/localization/BUILD
+++ b/frc971/control_loops/drivetrain/localization/BUILD
@@ -13,6 +13,7 @@
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
"//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/control_loops/drivetrain:hybrid_ekf",
+ "//frc971/control_loops/drivetrain:rio_localizer_inputs_fbs",
"//frc971/input:joystick_state_fbs",
"//frc971/vision:calibration_fbs",
"@org_tuxfamily_eigen//:eigen",
diff --git a/frc971/control_loops/drivetrain/localization/utils.cc b/frc971/control_loops/drivetrain/localization/utils.cc
index 1f4aadc..0986037 100644
--- a/frc971/control_loops/drivetrain/localization/utils.cc
+++ b/frc971/control_loops/drivetrain/localization/utils.cc
@@ -4,39 +4,62 @@
LocalizationUtils::LocalizationUtils(aos::EventLoop *event_loop)
: event_loop_(event_loop),
- output_fetcher_(event_loop->MakeFetcher<Output>("/drivetrain")),
+ output_fetcher_(event_loop->TryMakeFetcher<Output>("/drivetrain")),
position_fetcher_(event_loop->TryMakeFetcher<Position>("/drivetrain")),
+ combined_fetcher_(
+ event_loop->TryMakeFetcher<RioLocalizerInputs>("/drivetrain")),
clock_offset_fetcher_(
event_loop->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/aos")),
joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/roborio/aos")) {}
+namespace {
+template <typename T>
+Eigen::Vector2d GetVoltage(T &fetcher, aos::monotonic_clock::time_point now) {
+ fetcher.Fetch();
+ // Determine if the robot is likely to be disabled currently.
+ const bool disabled =
+ (fetcher.get() == nullptr) ||
+ (fetcher.context().monotonic_event_time + std::chrono::milliseconds(10) <
+ now);
+ return disabled ? Eigen::Vector2d::Zero()
+ : Eigen::Vector2d{fetcher->left_voltage(),
+ fetcher->right_voltage()};
+}
+} // namespace
Eigen::Vector2d LocalizationUtils::VoltageOrZero(
aos::monotonic_clock::time_point now) {
- output_fetcher_.Fetch();
- // Determine if the robot is likely to be disabled currently.
- const bool disabled = (output_fetcher_.get() == nullptr) ||
- (output_fetcher_.context().monotonic_event_time +
- std::chrono::milliseconds(10) <
- now);
- return disabled ? Eigen::Vector2d::Zero()
- : Eigen::Vector2d{output_fetcher_->left_voltage(),
- output_fetcher_->right_voltage()};
+ if (output_fetcher_.valid()) {
+ return GetVoltage(output_fetcher_, now);
+ } else {
+ CHECK(combined_fetcher_.valid());
+ return GetVoltage(combined_fetcher_, now);
+ }
}
+namespace {
+template <typename T>
+std::optional<Eigen::Vector2d> GetPosition(
+ T &fetcher, aos::monotonic_clock::time_point now) {
+ fetcher.Fetch();
+ const bool stale =
+ (fetcher.get() == nullptr) ||
+ (fetcher.context().monotonic_event_time + std::chrono::milliseconds(10) <
+ now);
+ return stale ? std::nullopt
+ : std::make_optional<Eigen::Vector2d>(fetcher->left_encoder(),
+ fetcher->right_encoder());
+}
+} // namespace
std::optional<Eigen::Vector2d> LocalizationUtils::Encoders(
aos::monotonic_clock::time_point now) {
- CHECK(position_fetcher_.valid());
- position_fetcher_.Fetch();
- const bool stale = (position_fetcher_.get() == nullptr) ||
- (position_fetcher_.context().monotonic_event_time +
- std::chrono::milliseconds(10) <
- now);
- return stale ? std::nullopt
- : std::make_optional<Eigen::Vector2d>(
- position_fetcher_->left_encoder(),
- position_fetcher_->right_encoder());
+ if (position_fetcher_.valid()) {
+ return GetPosition(position_fetcher_, now);
+ } else {
+ CHECK(combined_fetcher_.valid());
+ return GetPosition(combined_fetcher_, now);
+ }
}
bool LocalizationUtils::MaybeInAutonomous() {
diff --git a/frc971/control_loops/drivetrain/localization/utils.h b/frc971/control_loops/drivetrain/localization/utils.h
index 07f9aff..4d21e3b 100644
--- a/frc971/control_loops/drivetrain/localization/utils.h
+++ b/frc971/control_loops/drivetrain/localization/utils.h
@@ -7,6 +7,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+#include "frc971/control_loops/drivetrain/rio_localizer_inputs_generated.h"
#include "frc971/control_loops/pose.h"
#include "frc971/input/joystick_state_generated.h"
#include "frc971/vision/calibration_generated.h"
@@ -50,6 +51,8 @@
aos::EventLoop *const event_loop_;
aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
aos::Fetcher<frc971::control_loops::drivetrain::Position> position_fetcher_;
+ aos::Fetcher<frc971::control_loops::drivetrain::RioLocalizerInputs>
+ combined_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
};
diff --git a/frc971/control_loops/drivetrain/rio_localizer_inputs.fbs b/frc971/control_loops/drivetrain/rio_localizer_inputs.fbs
new file mode 100644
index 0000000..b9dc359
--- /dev/null
+++ b/frc971/control_loops/drivetrain/rio_localizer_inputs.fbs
@@ -0,0 +1,12 @@
+namespace frc971.control_loops.drivetrain;
+
+// This combines the drivetrian Position and Output messages
+// to be forwarded to the imu orin.
+table RioLocalizerInputs {
+ left_voltage:double (id: 0);
+ right_voltage:double (id: 1);
+ left_encoder:double (id: 2);
+ right_encoder:double (id: 3);
+}
+
+root_type RioLocalizerInputs;