Merge "Adjust the wheel radius and gear ratio for the drivetrain"
diff --git a/.bazelrc b/.bazelrc
index be65a4b..b12d5b2 100644
--- a/.bazelrc
+++ b/.bazelrc
@@ -22,13 +22,15 @@
# Shortcuts for selecting the target platform.
build:k8 --platforms=//tools/platforms:linux_x86
build:roborio --platforms=//tools/platforms:linux_roborio
+build:roborio --platform_suffix=-roborio
build:armv7 --platforms=//tools/platforms:linux_armv7
+build:armv7 --platform_suffix=-armv7
build:arm64 --platforms=//tools/platforms:linux_arm64
+build:arm64 --platform_suffix=-arm64
build:cortex-m4f --platforms=//tools/platforms:cortex_m4f
+build:cortex-m4f --platform_suffix=-cortex-m4f
build:rp2040 --platforms=//tools/platforms:rp2040
-
-# Without this, we end up rebuilding from scratch every time we change compilers. This is needed to make --cpu work (even though it shouldn't be used).
-build --crosstool_top=@//tools/cpp:toolchain --host_crosstool_top=@//tools/cpp:toolchain
+build:rp2040 --platform_suffix=-rp2040
build:asan --copt -fsanitize=address
build:asan --linkopt -fsanitize=address --linkopt -ldl
diff --git a/aos/events/simulated_network_bridge.cc b/aos/events/simulated_network_bridge.cc
index 4c9208e..c6ea811 100644
--- a/aos/events/simulated_network_bridge.cc
+++ b/aos/events/simulated_network_bridge.cc
@@ -228,6 +228,8 @@
// TODO(austin): Not cool. We want to actually forward these. This means
// we need a more sophisticated concept of what is running.
+ // TODO(james): This fails if multiple messages are sent on the same channel
+ // within the same callback.
LOG(WARNING) << "Not forwarding message on "
<< configuration::CleanedChannelToString(fetcher_->channel())
<< " because we aren't running. Set at "
diff --git a/aos/network/rawrtc.cc b/aos/network/rawrtc.cc
index 98f2448..c195a4b 100644
--- a/aos/network/rawrtc.cc
+++ b/aos/network/rawrtc.cc
@@ -148,8 +148,9 @@
void ScopedDataChannel::Close() {
CHECK(opened_);
- CHECK(!closed_);
- CHECK_RAWRTC(rawrtc_data_channel_close(data_channel_));
+ if (!closed_) {
+ CHECK_RAWRTC(rawrtc_data_channel_close(data_channel_));
+ }
}
void ScopedDataChannel::Send(const ::flatbuffers::DetachedBuffer &buffer) {
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index d846ce3..82f397c 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -52,6 +52,7 @@
"//y2020/control_loops/superstructure:hood_plotter",
"//y2020/control_loops/superstructure:turret_plotter",
"//y2021_bot3/control_loops/superstructure:superstructure_plotter",
+ "//y2022/control_loops/localizer:localizer_plotter",
],
)
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 2df65f6..a5203b0 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -32,8 +32,10 @@
'org_frc971/y2020/control_loops/superstructure/finisher_plotter'
import {plotTurret} from
'org_frc971/y2020/control_loops/superstructure/turret_plotter'
-import {plotLocalizer} from
+import {plotLocalizer as plot2020Localizer} from
'org_frc971/y2020/control_loops/drivetrain/localizer_plotter'
+import {plotLocalizer as plot2022Localizer} from
+ 'org_frc971/y2022/control_loops/localizer/localizer_plotter'
import {plotAccelerator} from
'org_frc971/y2020/control_loops/superstructure/accelerator_plotter'
import {plotHood} from
@@ -104,7 +106,8 @@
['Accelerator', new PlotState(plotDiv, plotAccelerator)],
['Hood', new PlotState(plotDiv, plotHood)],
['Turret', new PlotState(plotDiv, plotTurret)],
- ['2020 Localizer', new PlotState(plotDiv, plotLocalizer)],
+ ['2022 Localizer', new PlotState(plotDiv, plot2022Localizer)],
+ ['2020 Localizer', new PlotState(plotDiv, plot2020Localizer)],
['C++ Plotter', new PlotState(plotDiv, plotData)],
['Y2021 3rd Robot Superstructure', new PlotState(plotDiv, plotSuperstructure)],
]);
@@ -154,4 +157,4 @@
plotSelect.value = getDefaultPlot();
// Force the event to occur once at the start.
plotSelect.dispatchEvent(new Event('input'));
-});
\ No newline at end of file
+});
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 62b77c4..66a3dcd 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -35,7 +35,7 @@
localizer_control_fetcher_(
event_loop->MakeFetcher<LocalizerControl>("/drivetrain")),
imu_values_fetcher_(
- event_loop->MakeFetcher<::frc971::IMUValuesBatch>("/drivetrain")),
+ event_loop->TryMakeFetcher<::frc971::IMUValuesBatch>("/drivetrain")),
gyro_reading_fetcher_(
event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
"/drivetrain")),
@@ -52,7 +52,9 @@
event_loop->OnRun([this]() {
// On the first fetch, make sure that we are caught all the way up to the
// present.
- imu_values_fetcher_.Fetch();
+ if (imu_values_fetcher_.valid()) {
+ imu_values_fetcher_.Fetch();
+ }
});
if (dt_config.is_simulated) {
down_estimator_.assume_perfect_gravity();
@@ -117,7 +119,7 @@
break;
}
- while (imu_values_fetcher_.FetchNext()) {
+ while (imu_values_fetcher_.valid() && imu_values_fetcher_.FetchNext()) {
CHECK(imu_values_fetcher_->has_readings());
last_gyro_time_ = monotonic_now;
for (const IMUValues *value : *imu_values_fetcher_->readings()) {
@@ -138,7 +140,7 @@
}
bool got_imu_reading = false;
- if (imu_values_fetcher_.get() != nullptr) {
+ if (imu_values_fetcher_.valid() && imu_values_fetcher_.get() != nullptr) {
imu_zeroer_.ProcessMeasurements();
got_imu_reading = true;
CHECK(imu_values_fetcher_->has_readings());
@@ -202,7 +204,9 @@
break;
}
- ready_ = imu_zeroer_.Zeroed();
+ ready_ = dt_config_.gyro_type == GyroType::SPARTAN_GYRO ||
+ dt_config_.gyro_type == GyroType::FLIPPED_SPARTAN_GYRO ||
+ imu_zeroer_.Zeroed();
// TODO(james): How aggressively can we fault here? If we fault to
// aggressively, we might have issues during startup if wpilib_interface takes
@@ -211,11 +215,18 @@
last_gyro_rate_ = 0.0;
}
- localizer_->Update(
- {last_last_voltage_(kLeftVoltage), last_last_voltage_(kRightVoltage)},
- monotonic_now, position->left_encoder(), position->right_encoder(),
- down_estimator_.avg_recent_yaw_rates(),
- down_estimator_.avg_recent_accel());
+ if (imu_values_fetcher_.valid()) {
+ localizer_->Update(
+ {last_last_voltage_(kLeftVoltage), last_last_voltage_(kRightVoltage)},
+ monotonic_now, position->left_encoder(), position->right_encoder(),
+ down_estimator_.avg_recent_yaw_rates(),
+ down_estimator_.avg_recent_accel());
+ } else {
+ localizer_->Update(
+ {last_last_voltage_(kLeftVoltage), last_last_voltage_(kRightVoltage)},
+ monotonic_now, position->left_encoder(), position->right_encoder(),
+ last_gyro_rate_, Eigen::Vector3d::Zero());
+ }
// If we get a new message setting the absolute position, then reset the
// localizer.
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 4b5138e..a973552 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -93,16 +93,20 @@
}
DrivetrainSimulation::DrivetrainSimulation(
- ::aos::EventLoop *event_loop, const DrivetrainConfig<double> &dt_config)
+ ::aos::EventLoop *event_loop, ::aos::EventLoop *imu_event_loop,
+ const DrivetrainConfig<double> &dt_config,
+ aos::monotonic_clock::duration imu_read_period)
: event_loop_(event_loop),
+ imu_event_loop_(imu_event_loop),
robot_state_fetcher_(event_loop_->MakeFetcher<::aos::RobotState>("/aos")),
drivetrain_position_sender_(
event_loop_
->MakeSender<::frc971::control_loops::drivetrain::Position>(
"/drivetrain")),
drivetrain_truth_sender_(
- event_loop_->MakeSender<::frc971::control_loops::drivetrain::Status>(
- "/drivetrain/truth")),
+ event_loop_
+ ->TryMakeSender<::frc971::control_loops::drivetrain::Status>(
+ "/drivetrain/truth")),
drivetrain_output_fetcher_(
event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Output>(
"/drivetrain")),
@@ -110,7 +114,7 @@
event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
imu_sender_(
- event_loop->MakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
+ event_loop->TryMakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
dt_config_(dt_config),
drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
@@ -121,6 +125,14 @@
StateFeedbackHybridPlant<2, 2, 2>,
HybridKalman<2, 2, 2>>(
dt_config_.make_hybrid_drivetrain_velocity_loop()))) {
+ if (imu_event_loop_ != nullptr) {
+ CHECK(!imu_sender_);
+ imu_sender_ =
+ imu_event_loop_->MakeSender<::frc971::IMUValuesBatch>("/localizer");
+ gyro_sender_ =
+ event_loop_->MakeSender<::frc971::sensors::GyroReading>("/drivetrain");
+ }
+ CHECK(imu_sender_);
Reinitialize();
last_U_.setZero();
event_loop_->AddPhasedLoop(
@@ -147,9 +159,7 @@
SendImuMessage();
},
dt_config_.dt);
- // TODO(milind): We should be able to get IMU readings at 1 kHz instead of 2.
- event_loop_->AddPhasedLoop([this](int) { ReadImu(); },
- frc971::controls::kLoopFrequency);
+ event_loop_->AddPhasedLoop([this](int) { ReadImu(); }, imu_read_period);
}
void DrivetrainSimulation::Reinitialize() {
@@ -163,6 +173,9 @@
}
void DrivetrainSimulation::SendTruthMessage() {
+ if (!drivetrain_truth_sender_) {
+ return;
+ }
auto builder = drivetrain_truth_sender_.MakeBuilder();
auto status_builder =
builder.MakeBuilder<frc971::control_loops::drivetrain::Status>();
@@ -178,15 +191,16 @@
const double right_encoder = GetRightPosition();
if (send_messages_) {
- ::aos::Sender<::frc971::control_loops::drivetrain::Position>::Builder
- builder = drivetrain_position_sender_.MakeBuilder();
- frc971::control_loops::drivetrain::Position::Builder position_builder =
- builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+ flatbuffers::FlatBufferBuilder fbb;
+ frc971::control_loops::drivetrain::Position::Builder position_builder(fbb);
position_builder.add_left_encoder(left_encoder);
position_builder.add_right_encoder(right_encoder);
position_builder.add_left_shifter_position(left_gear_high_ ? 1.0 : 0.0);
position_builder.add_right_shifter_position(right_gear_high_ ? 1.0 : 0.0);
- CHECK_EQ(builder.Send(position_builder.Finish()),
+ fbb.Finish(position_builder.Finish());
+ aos::FlatbufferDetachedBuffer<frc971::control_loops::drivetrain::Position>
+ position(fbb.Release());
+ CHECK_EQ(drivetrain_position_sender_.Send(position),
aos::RawSender::Error::kOk);
}
}
@@ -204,17 +218,19 @@
(dt_config_.robot_radius * 2.0));
// Acceleration due to gravity, in m/s/s.
- constexpr double kG = 9.807;
+ constexpr double kG = 9.80665;
const Eigen::Vector3d accel =
dt_config_.imu_transform.inverse() *
Eigen::Vector3d(last_acceleration_.x() / kG, last_acceleration_.y() / kG,
- 1.0);
+ last_acceleration_.z() + 1.0);
const int64_t timestamp =
std::chrono::duration_cast<std::chrono::nanoseconds>(
event_loop_->monotonic_now().time_since_epoch())
.count();
+ last_yaw_rate_ = gyro.z();
imu_readings_.push({.gyro = gyro,
.accel = accel,
+ .encoders = {GetLeftPosition(), GetRightPosition()},
.timestamp = timestamp,
.faulted = imu_faulted_});
}
@@ -224,16 +240,15 @@
return;
}
+ flatbuffers::FlatBufferBuilder fbb;
std::vector<flatbuffers::Offset<IMUValues>> imu_values;
- auto builder = imu_sender_.MakeBuilder();
// Send all the IMU readings and pop the ones we have sent
while (!imu_readings_.empty()) {
const auto imu_reading = imu_readings_.front();
imu_readings_.pop();
- frc971::ADIS16470DiagStat::Builder diag_stat_builder =
- builder.MakeBuilder<frc971::ADIS16470DiagStat>();
+ frc971::ADIS16470DiagStat::Builder diag_stat_builder(fbb);
diag_stat_builder.add_clock_error(false);
diag_stat_builder.add_memory_failure(imu_reading.faulted);
diag_stat_builder.add_sensor_failure(false);
@@ -244,8 +259,7 @@
const auto diag_stat_offset = diag_stat_builder.Finish();
- frc971::IMUValues::Builder imu_builder =
- builder.MakeBuilder<frc971::IMUValues>();
+ frc971::IMUValues::Builder imu_builder(fbb);
imu_builder.add_self_test_diag_stat(diag_stat_offset);
imu_builder.add_gyro_x(imu_reading.gyro.x());
@@ -257,17 +271,34 @@
imu_builder.add_accelerometer_z(imu_reading.accel.z());
imu_builder.add_monotonic_timestamp_ns(imu_reading.timestamp);
+ if (imu_event_loop_ != nullptr) {
+ imu_builder.add_pico_timestamp_us(imu_reading.timestamp / 1000);
+ imu_builder.add_data_counter(imu_data_counter_++);
+ imu_builder.add_checksum_failed(false);
+ imu_builder.add_left_encoder(imu_reading.encoders(0));
+ imu_builder.add_right_encoder(imu_reading.encoders(1));
+ }
+
imu_values.push_back(imu_builder.Finish());
}
flatbuffers::Offset<
flatbuffers::Vector<flatbuffers::Offset<frc971::IMUValues>>>
- imu_values_offset = builder.fbb()->CreateVector(imu_values);
- frc971::IMUValuesBatch::Builder imu_values_batch_builder =
- builder.MakeBuilder<frc971::IMUValuesBatch>();
+ imu_values_offset = fbb.CreateVector(imu_values);
+ frc971::IMUValuesBatch::Builder imu_values_batch_builder(fbb);
imu_values_batch_builder.add_readings(imu_values_offset);
- CHECK_EQ(builder.Send(imu_values_batch_builder.Finish()),
- aos::RawSender::Error::kOk);
+ fbb.Finish(imu_values_batch_builder.Finish());
+ aos::FlatbufferDetachedBuffer<frc971::IMUValuesBatch> message = fbb.Release();
+ CHECK_EQ(imu_sender_.Send(message), aos::RawSender::Error::kOk);
+ if (gyro_sender_) {
+ auto builder = gyro_sender_.MakeBuilder();
+ sensors::GyroReading::Builder reading_builder =
+ builder.MakeBuilder<sensors::GyroReading>();
+ reading_builder.add_angle(state_(2));
+ reading_builder.add_velocity(last_yaw_rate_);
+ CHECK_EQ(builder.Send(reading_builder.Finish()),
+ aos::RawSender::Error::kOk);
+ }
}
// Simulates the drivetrain moving for one timestep.
@@ -331,6 +362,12 @@
// TODO(james): Allow inputting arbitrary calibrations, e.g., for testing
// situations where the IMU is not perfectly flat in the CG of the robot.
last_acceleration_ << (Xdot(3, 0) + Xdot(4, 0)) / 2.0, centripetal_accel, 0.0;
+ double accel_disturbance =
+ std::sin(10.0 * 2 * M_PI *
+ aos::time::DurationInSeconds(
+ event_loop_->monotonic_now().time_since_epoch())) *
+ accel_sin_wave_magnitude_;
+ last_acceleration_.z() += accel_disturbance;
}
void DrivetrainSimulation::MaybePlot() {
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index b98711b..356eadf 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -13,6 +13,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/wpilib/imu_batch_generated.h"
+#include "frc971/queues/gyro_generated.h"
namespace frc971 {
namespace control_loops {
@@ -50,7 +51,13 @@
// Constructs a motor simulation.
// TODO(aschuh) Do we want to test the clutch one too?
DrivetrainSimulation(::aos::EventLoop *event_loop,
- const DrivetrainConfig<double> &dt_config);
+ ::aos::EventLoop *imu_event_loop,
+ const DrivetrainConfig<double> &dt_config,
+ aos::monotonic_clock::duration imu_read_period =
+ frc971::controls::kLoopFrequency);
+ DrivetrainSimulation(::aos::EventLoop *event_loop,
+ const DrivetrainConfig<double> &dt_config)
+ : DrivetrainSimulation(event_loop, nullptr, dt_config) {}
// Resets the plant.
void Reinitialize();
@@ -87,11 +94,17 @@
}
void set_imu_faulted(const bool fault_imu) { imu_faulted_ = fault_imu; }
+ void set_accel_sin_magnitude(double magnitude) {
+ accel_sin_wave_magnitude_ = magnitude;
+ }
private:
struct ImuReading {
Eigen::Vector3d gyro;
Eigen::Vector3d accel;
+ // On the 2022 robot, encoders are read as part of the same procedure that
+ // reads the IMU.
+ Eigen::Vector2d encoders;
int64_t timestamp;
bool faulted;
};
@@ -109,6 +122,7 @@
void Simulate();
::aos::EventLoop *event_loop_;
+ ::aos::EventLoop *imu_event_loop_;
::aos::Fetcher<::aos::RobotState> robot_state_fetcher_;
::aos::Sender<::frc971::control_loops::drivetrain::Position>
@@ -120,9 +134,12 @@
::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
::aos::Sender<::frc971::IMUValuesBatch> imu_sender_;
+ ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
bool imu_faulted_ = false;
+ double last_yaw_rate_ = 0.0;
+ int imu_data_counter_ = 0;
std::queue<ImuReading> imu_readings_;
DrivetrainConfig<double> dt_config_;
@@ -153,6 +170,8 @@
::std::vector<double> trajectory_y_;
bool send_messages_ = true;
+ // Magnitude of sine wave to feed into the measured accelerations.
+ double accel_sin_wave_magnitude_ = 0.0;
};
} // namespace testing
diff --git a/frc971/raspi/rootfs/README.md b/frc971/raspi/rootfs/README.md
index f8d9971..b972038 100644
--- a/frc971/raspi/rootfs/README.md
+++ b/frc971/raspi/rootfs/README.md
@@ -69,6 +69,6 @@
* Download the code:
Once this is completed, you can boot the pi with the newly flashed SD
card, and download the code to the pi using:
- `bazel run -c opt --cpu=armv7 //y2022:pi_download_stripped -- PI_IP_ADDR
+ `bazel run -c opt --config=armv7 //y2022:pi_download_stripped -- PI_IP_ADDR
where PI_IP_ADDR is the IP address of the target pi, e.g., 10.9.71.101
diff --git a/platform_mappings b/platform_mappings
deleted file mode 100644
index 1816850..0000000
--- a/platform_mappings
+++ /dev/null
@@ -1,38 +0,0 @@
-# https://docs.bazel.build/versions/master/platforms-intro.html#platform-mappings
-platforms:
- //tools/platforms:linux_x86
- --cpu=k8
-
- //tools/platforms:linux_roborio
- --cpu=roborio
-
- //tools/platforms:linux_armv7
- --cpu=armv7
-
- //tools/platforms:cortex_m4f
- --cpu=cortex-m4f
-
- //tools/platforms:rp2040
- --cpu=rp2040
-
- //tools/platforms:linux_arm64
- --cpu=arm64
-
-flags:
- --cpu=k8
- //tools/platforms:linux_x86
-
- --cpu=roborio
- //tools/platforms:linux_roborio
-
- --cpu=armv7
- //tools/platforms:linux_armv7
-
- --cpu=cortex-m4f
- //tools/platforms:cortex_m4f
-
- --cpu=rp2040
- //tools/platforms:rp2040
-
- --cpu=arm64
- //tools/platforms:linux_arm64
diff --git a/third_party/abseil/absl/copts/configure_copts.bzl b/third_party/abseil/absl/copts/configure_copts.bzl
index 4d34254..46dc156 100644
--- a/third_party/abseil/absl/copts/configure_copts.bzl
+++ b/third_party/abseil/absl/copts/configure_copts.bzl
@@ -63,7 +63,7 @@
# These configs have consistent flags to enable HWAES intsructions.
cpu_configs = [
"ppc",
- "k8",
+ #"k8",
"darwin_x86_64",
"darwin",
"x64_windows_msvc",
@@ -74,3 +74,7 @@
name = "cpu_%s" % cpu,
values = {"cpu": cpu},
)
+ native.config_setting(
+ name = "cpu_k8",
+ constraint_values = ["@platforms//cpu:x86_64"],
+ )
diff --git a/third_party/bazel-toolchain/bazel_tools_changes/tools/cpp/unix_cc_toolchain_config.bzl b/third_party/bazel-toolchain/bazel_tools_changes/tools/cpp/unix_cc_toolchain_config.bzl
index 8de828f..0044dce 100755
--- a/third_party/bazel-toolchain/bazel_tools_changes/tools/cpp/unix_cc_toolchain_config.bzl
+++ b/third_party/bazel-toolchain/bazel_tools_changes/tools/cpp/unix_cc_toolchain_config.bzl
@@ -160,6 +160,17 @@
action_configs.append(llvm_cov_action)
+ objcopy_embed_data_action = action_config(
+ action_name = "objcopy_embed_data",
+ tools = [
+ tool(
+ path = ctx.attr.tool_paths["objcopy"],
+ ),
+ ],
+ )
+
+ action_configs.append(objcopy_embed_data_action)
+
supports_pic_feature = feature(
name = "supports_pic",
enabled = True,
diff --git a/third_party/pico-sdk/hex.bzl b/third_party/pico-sdk/hex.bzl
index 5e9dfae..31c951c 100644
--- a/third_party/pico-sdk/hex.bzl
+++ b/third_party/pico-sdk/hex.bzl
@@ -7,7 +7,7 @@
executable = True,
output_to_bindir = True,
target_compatible_with = target_compatible_with,
- toolchains = ["@bazel_tools//tools/cpp:current_cc_toolchain"],
+ toolchains = ["//tools/cpp:cc_toolchain_make_variables"],
)
def uf2_from_elf(name, target_compatible_with = None):
diff --git a/tools/cpp/BUILD b/tools/cpp/BUILD
index db5fa06..5c36559 100644
--- a/tools/cpp/BUILD
+++ b/tools/cpp/BUILD
@@ -1,19 +1,11 @@
load(":toolchain_config.bzl", "cc_toolchain_config")
+load(":toolchain_make_variables.bzl", "cc_toolchain_make_variables")
package(default_visibility = ["//visibility:public"])
-cc_toolchain_suite(
- name = "toolchain",
- toolchains = {
- "k8": "@llvm_toolchain//:cc-clang-x86_64-linux",
- "armv7": "@llvm_toolchain//:cc-clang-armv7-linux",
- "arm64": "@llvm_toolchain//:cc-clang-aarch64-linux",
- "roborio": ":cc-compiler-roborio",
- "cortex-m4f": ":cc-compiler-cortex-m4f",
- "rp2040": ":cc-compiler-rp2040",
- },
- visibility = ["//visibility:public"],
-)
+# Use this instead of @bazel_tools//tools/cpp:current_cc_toolchain.
+# This one supports platforms.
+cc_toolchain_make_variables(name = "cc_toolchain_make_variables")
[
cc_toolchain_config(
diff --git a/tools/cpp/toolchain_make_variables.bzl b/tools/cpp/toolchain_make_variables.bzl
new file mode 100644
index 0000000..7c06e1d
--- /dev/null
+++ b/tools/cpp/toolchain_make_variables.bzl
@@ -0,0 +1,39 @@
+load("@bazel_tools//tools/cpp:toolchain_utils.bzl", "find_cpp_toolchain")
+
+def _cc_toolchain_make_variables_impl(ctx):
+ """Supports make variables for toolchains in a platforms setup.
+
+ The upstream @bazel_tools//tools/cpp:current_cc_toolchain target on its own
+ doesn't appear to work with platforms. It only works when using --cpu.
+ """
+ toolchain = find_cpp_toolchain(ctx)
+
+ feature_configuration = cc_common.configure_features(
+ ctx = ctx,
+ cc_toolchain = toolchain,
+ requested_features = ctx.features,
+ unsupported_features = ctx.disabled_features,
+ )
+ objcopy = cc_common.get_tool_for_action(
+ feature_configuration = feature_configuration,
+ action_name = "objcopy_embed_data",
+ )
+
+ return [
+ platform_common.TemplateVariableInfo({
+ "OBJCOPY": objcopy,
+ }),
+ DefaultInfo(files = toolchain.all_files),
+ ]
+
+cc_toolchain_make_variables = rule(
+ implementation = _cc_toolchain_make_variables_impl,
+ attrs = {
+ # This is a dependency of find_cpp_toolchain().
+ "_toolchain": attr.label(
+ default = Label("@bazel_tools//tools/cpp:current_cc_toolchain"),
+ ),
+ },
+ fragments = ["cpp"],
+ toolchains = ["@bazel_tools//tools/cpp:toolchain_type"],
+)
diff --git a/y2022/BUILD b/y2022/BUILD
index 78cb86c..97d42a1 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -29,6 +29,7 @@
"//y2020/vision:calibration",
"//y2022/vision:viewer",
"//y2022/localizer:imu_main",
+ "//y2022/control_loops/localizer:localizer_main",
],
data = [
":config",
@@ -58,6 +59,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
+ ":config_imu",
":config_logger",
":config_pi1",
":config_pi2",
@@ -99,6 +101,25 @@
]
aos_config(
+ name = "config_imu",
+ src = "y2022_imu.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/network:remote_message_fbs",
+ "//y2022/control_loops/localizer:localizer_status_fbs",
+ "//y2022/control_loops/localizer:localizer_output_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:config",
+ "//frc971/control_loops/drivetrain:config",
+ ],
+)
+
+aos_config(
name = "config_logger",
src = "y2022_logger.json",
flatbuffers = [
diff --git a/y2022/control_loops/drivetrain/BUILD b/y2022/control_loops/drivetrain/BUILD
index 2858dc7..a1f1e47 100644
--- a/y2022/control_loops/drivetrain/BUILD
+++ b/y2022/control_loops/drivetrain/BUILD
@@ -1,3 +1,5 @@
+load("//aos:config.bzl", "aos_config")
+
genrule(
name = "genrule_drivetrain",
outs = [
@@ -69,6 +71,19 @@
],
)
+cc_library(
+ name = "localizer",
+ srcs = ["localizer.cc"],
+ hdrs = ["localizer.h"],
+ deps = [
+ "//aos/events:event_loop",
+ "//aos/network:message_bridge_server_fbs",
+ "//frc971/control_loops/drivetrain:hybrid_ekf",
+ "//frc971/control_loops/drivetrain:localizer",
+ "//y2022/control_loops/localizer:localizer_output_fbs",
+ ],
+)
+
cc_binary(
name = "drivetrain",
srcs = [
@@ -78,8 +93,40 @@
visibility = ["//visibility:public"],
deps = [
":drivetrain_base",
+ ":localizer",
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:drivetrain_lib",
],
)
+
+aos_config(
+ name = "simulation_config",
+ src = "drivetrain_simulation_config.json",
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/drivetrain:simulation_channels",
+ "//y2022:config",
+ ],
+)
+
+cc_test(
+ name = "localizer_test",
+ srcs = ["localizer_test.cc"],
+ data = [":simulation_config"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":drivetrain_base",
+ ":localizer",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_writer",
+ "//aos/network:team_number",
+ "//aos/network:testing_time_converter",
+ "//frc971/control_loops:control_loop_test",
+ "//frc971/control_loops:team_number_test_environment",
+ "//frc971/control_loops/drivetrain:drivetrain_lib",
+ "//frc971/control_loops/drivetrain:drivetrain_test_lib",
+ "//y2022/control_loops/localizer:localizer_output_fbs",
+ ],
+)
diff --git a/y2022/control_loops/drivetrain/drivetrain_base.cc b/y2022/control_loops/drivetrain/drivetrain_base.cc
index cbc9c42..93233d2 100644
--- a/y2022/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2022/control_loops/drivetrain/drivetrain_base.cc
@@ -25,7 +25,7 @@
static DrivetrainConfig<double> kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
drivetrain::MakeDrivetrainLoop,
diff --git a/y2022/control_loops/drivetrain/drivetrain_main.cc b/y2022/control_loops/drivetrain/drivetrain_main.cc
index e29f950..ecdcdbb 100644
--- a/y2022/control_loops/drivetrain/drivetrain_main.cc
+++ b/y2022/control_loops/drivetrain/drivetrain_main.cc
@@ -4,6 +4,7 @@
#include "aos/init.h"
#include "frc971/control_loops/drivetrain/drivetrain.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
+#include "y2022/control_loops/drivetrain/localizer.h"
using ::frc971::control_loops::drivetrain::DrivetrainLoop;
@@ -14,7 +15,7 @@
aos::configuration::ReadConfig("config.json");
::aos::ShmEventLoop event_loop(&config.message());
- ::frc971::control_loops::drivetrain::DeadReckonEkf localizer(
+ ::y2022::control_loops::drivetrain::Localizer localizer(
&event_loop,
::y2022::control_loops::drivetrain::GetDrivetrainConfig());
std::unique_ptr<DrivetrainLoop> drivetrain = std::make_unique<DrivetrainLoop>(
diff --git a/y2022/control_loops/drivetrain/drivetrain_simulation_config.json b/y2022/control_loops/drivetrain/drivetrain_simulation_config.json
new file mode 100644
index 0000000..37c1bb7
--- /dev/null
+++ b/y2022/control_loops/drivetrain/drivetrain_simulation_config.json
@@ -0,0 +1,6 @@
+{
+ "imports": [
+ "../../y2022.json",
+ "../../../frc971/control_loops/drivetrain/drivetrain_simulation_channels.json"
+ ]
+}
diff --git a/y2022/control_loops/drivetrain/localizer.cc b/y2022/control_loops/drivetrain/localizer.cc
new file mode 100644
index 0000000..3d02312
--- /dev/null
+++ b/y2022/control_loops/drivetrain/localizer.cc
@@ -0,0 +1,102 @@
+#include "y2022/control_loops/drivetrain/localizer.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace drivetrain {
+
+Localizer::Localizer(
+ aos::EventLoop *event_loop,
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config)
+ : event_loop_(event_loop),
+ dt_config_(dt_config),
+ ekf_(dt_config),
+ localizer_output_fetcher_(
+ event_loop_->MakeFetcher<frc971::controls::LocalizerOutput>(
+ "/localizer")),
+ clock_offset_fetcher_(
+ event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
+ "/aos")) {
+ ekf_.set_ignore_accel(true);
+
+ event_loop->OnRun([this, event_loop]() {
+ ekf_.ResetInitialState(event_loop->monotonic_now(),
+ HybridEkf::State::Zero(), ekf_.P());
+ });
+
+ target_selector_.set_has_target(false);
+}
+
+void Localizer::Reset(
+ aos::monotonic_clock::time_point t,
+ const frc971::control_loops::drivetrain::HybridEkf<double>::State &state) {
+ // Go through and clear out all of the fetchers so that we don't get behind.
+ localizer_output_fetcher_.Fetch();
+ ekf_.ResetInitialState(t, state.cast<float>(), ekf_.P());
+}
+
+void Localizer::Update(const Eigen::Matrix<double, 2, 1> &U,
+ aos::monotonic_clock::time_point now,
+ double left_encoder, double right_encoder,
+ double gyro_rate, const Eigen::Vector3d &accel) {
+ ekf_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate,
+ U.cast<float>(), accel.cast<float>(), now);
+ if (localizer_output_fetcher_.Fetch()) {
+ clock_offset_fetcher_.Fetch();
+ bool message_bridge_connected = true;
+ std::chrono::nanoseconds monotonic_offset{0};
+ if (clock_offset_fetcher_.get() != nullptr) {
+ for (const auto connection : *clock_offset_fetcher_->connections()) {
+ if (connection->has_node() && connection->node()->has_name() &&
+ connection->node()->name()->string_view() == "imu") {
+ if (connection->has_monotonic_offset()) {
+ monotonic_offset =
+ std::chrono::nanoseconds(connection->monotonic_offset());
+ } else {
+ // If we don't have a monotonic offset, that means we aren't
+ // connected, in which case we should break the loop but shouldn't
+ // populate the offset.
+ message_bridge_connected = false;
+ }
+ break;
+ }
+ }
+ }
+ if (!message_bridge_connected) {
+ return;
+ }
+ aos::monotonic_clock::time_point capture_time(
+ std::chrono::nanoseconds(
+ localizer_output_fetcher_->monotonic_timestamp_ns()) -
+ monotonic_offset);
+ // TODO: Finish implementing simple x/y/theta updater with state_at_capture.
+ // TODO: Implement turret/camera processing logic on pi side.
+ const std::optional<State> state_at_capture =
+ ekf_.LastStateBeforeTime(capture_time);
+ Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> H;
+ H.setZero();
+ H(0, StateIdx::kX) = 1;
+ H(1, StateIdx::kY) = 1;
+ H(2, StateIdx::kTheta) = 1;
+ const Eigen::Vector3f Z{
+ static_cast<float>(localizer_output_fetcher_->x()),
+ static_cast<float>(localizer_output_fetcher_->y()),
+ static_cast<float>(localizer_output_fetcher_->theta())};
+ Eigen::Matrix3f R = Eigen::Matrix3f::Zero();
+ R.diagonal() << 0.01, 0.01, 1e-4;
+ const Input U_correct = ekf_.MostRecentInput();
+ ekf_.Correct(
+ Eigen::Vector3f::Zero(), &U_correct, {},
+ [H, state_at_capture, Z](const State &,
+ const Input &) -> Eigen::Vector3f {
+ Eigen::Vector3f error = H * state_at_capture.value() - Z;
+ error(2) = aos::math::NormalizeAngle(error(2));
+ return error;
+ },
+ [H](const State &) { return H; }, R, now);
+ }
+}
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2022
diff --git a/y2022/control_loops/drivetrain/localizer.h b/y2022/control_loops/drivetrain/localizer.h
new file mode 100644
index 0000000..50d083a
--- /dev/null
+++ b/y2022/control_loops/drivetrain/localizer.h
@@ -0,0 +1,80 @@
+#ifndef Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
+#define Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
+
+#include <string_view>
+
+#include "aos/events/event_loop.h"
+#include "frc971/control_loops/drivetrain/hybrid_ekf.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
+#include "y2022/control_loops/localizer/localizer_output_generated.h"
+#include "aos/network/message_bridge_server_generated.h"
+
+namespace y2022 {
+namespace control_loops {
+namespace drivetrain {
+
+// This class handles the localization for the 2022 robot. Rather than actually
+// doing any work on the roborio, we farm all the localization out to a
+// raspberry pi and it then sends out LocalizerOutput messages that we treat as
+// measurement updates. See //y2022/control_loops/localizer.
+// TODO(james): Needs tests. Should refactor out some of the code from the 2020
+// localizer test.
+class Localizer : public frc971::control_loops::drivetrain::LocalizerInterface {
+ public:
+ typedef frc971::control_loops::TypedPose<float> Pose;
+ typedef frc971::control_loops::drivetrain::HybridEkf<float> HybridEkf;
+ typedef typename HybridEkf::State State;
+ typedef typename HybridEkf::StateIdx StateIdx;
+ typedef typename HybridEkf::StateSquare StateSquare;
+ typedef typename HybridEkf::Input Input;
+ typedef typename HybridEkf::Output Output;
+ Localizer(aos::EventLoop *event_loop,
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ &dt_config);
+ frc971::control_loops::drivetrain::HybridEkf<double>::State Xhat()
+ const override {
+ return ekf_.X_hat().cast<double>();
+ }
+ frc971::control_loops::drivetrain::TrivialTargetSelector *target_selector()
+ override {
+ return &target_selector_;
+ }
+
+ void Update(const ::Eigen::Matrix<double, 2, 1> &U,
+ aos::monotonic_clock::time_point now, double left_encoder,
+ double right_encoder, double gyro_rate,
+ const Eigen::Vector3d &accel) override;
+
+ void Reset(aos::monotonic_clock::time_point t,
+ const frc971::control_loops::drivetrain::HybridEkf<double>::State
+ &state) override;
+
+ void ResetPosition(aos::monotonic_clock::time_point t, double x, double y,
+ double theta, double /*theta_override*/,
+ bool /*reset_theta*/) override {
+ const double left_encoder = ekf_.X_hat(StateIdx::kLeftEncoder);
+ const double right_encoder = ekf_.X_hat(StateIdx::kRightEncoder);
+ ekf_.ResetInitialState(t,
+ (HybridEkf::State() << x, y, theta, left_encoder, 0,
+ right_encoder, 0, 0, 0, 0, 0, 0)
+ .finished(),
+ ekf_.P());
+ }
+
+ private:
+ aos::EventLoop *const event_loop_;
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ HybridEkf ekf_;
+
+ aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
+ aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
+
+ // Target selector to allow us to satisfy the LocalizerInterface requirements.
+ frc971::control_loops::drivetrain::TrivialTargetSelector target_selector_;
+};
+
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2022
+
+#endif // Y2022_CONTROL_LOOPS_DRIVETRAIN_LOCALIZER_H_
diff --git a/y2022/control_loops/drivetrain/localizer_test.cc b/y2022/control_loops/drivetrain/localizer_test.cc
new file mode 100644
index 0000000..87fc3fd
--- /dev/null
+++ b/y2022/control_loops/drivetrain/localizer_test.cc
@@ -0,0 +1,208 @@
+#include "y2022/control_loops/drivetrain/localizer.h"
+
+#include <queue>
+
+#include "aos/events/logging/log_writer.h"
+#include "aos/network/message_bridge_server_generated.h"
+#include "aos/network/team_number.h"
+#include "aos/network/testing_time_converter.h"
+#include "frc971/control_loops/control_loop_test.h"
+#include "frc971/control_loops/drivetrain/drivetrain.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+#include "frc971/control_loops/team_number_test_environment.h"
+#include "y2022/control_loops/localizer/localizer_output_generated.h"
+#include "gtest/gtest.h"
+#include "y2022/control_loops/drivetrain/drivetrain_base.h"
+
+DEFINE_string(output_folder, "",
+ "If set, logs all channels to the provided logfile.");
+
+namespace y2022 {
+namespace control_loops {
+namespace drivetrain {
+namespace testing {
+
+using frc971::control_loops::drivetrain::DrivetrainConfig;
+using frc971::control_loops::drivetrain::Goal;
+using frc971::control_loops::drivetrain::LocalizerControl;
+
+namespace {
+DrivetrainConfig<double> GetTest2022DrivetrainConfig() {
+ DrivetrainConfig<double> config = GetDrivetrainConfig();
+ return config;
+}
+}
+
+namespace chrono = std::chrono;
+using aos::monotonic_clock;
+using frc971::control_loops::drivetrain::DrivetrainLoop;
+using frc971::control_loops::drivetrain::testing::DrivetrainSimulation;
+
+// TODO(james): Make it so this actually tests the full system of the localizer.
+class LocalizedDrivetrainTest : public frc971::testing::ControlLoopTest {
+ protected:
+ // We must use the 2022 drivetrain config so that we don't have to deal
+ // with shifting:
+ LocalizedDrivetrainTest()
+ : frc971::testing::ControlLoopTest(
+ aos::configuration::ReadConfig(
+ "y2022/control_loops/drivetrain/simulation_config.json"),
+ GetTest2022DrivetrainConfig().dt),
+ roborio_(aos::configuration::GetNode(configuration(), "roborio")),
+ imu_(aos::configuration::GetNode(configuration(), "imu")),
+ test_event_loop_(MakeEventLoop("test", roborio_)),
+ imu_test_event_loop_(MakeEventLoop("test", imu_)),
+ drivetrain_goal_sender_(
+ test_event_loop_->MakeSender<Goal>("/drivetrain")),
+ localizer_output_sender_(
+ imu_test_event_loop_->MakeSender<frc971::controls::LocalizerOutput>(
+ "/localizer")),
+ drivetrain_goal_fetcher_(
+ test_event_loop_->MakeFetcher<Goal>("/drivetrain")),
+ drivetrain_status_fetcher_(
+ test_event_loop_
+ ->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
+ localizer_control_sender_(
+ test_event_loop_->MakeSender<LocalizerControl>("/drivetrain")),
+ drivetrain_event_loop_(MakeEventLoop("drivetrain", roborio_)),
+ dt_config_(GetTest2022DrivetrainConfig()),
+ localizer_(drivetrain_event_loop_.get(), dt_config_),
+ drivetrain_(dt_config_, drivetrain_event_loop_.get(), &localizer_),
+ drivetrain_plant_event_loop_(MakeEventLoop("plant", roborio_)),
+ drivetrain_plant_imu_event_loop_(MakeEventLoop("plant", imu_)),
+ drivetrain_plant_(drivetrain_plant_event_loop_.get(),
+ drivetrain_plant_imu_event_loop_.get(), dt_config_,
+ std::chrono::microseconds(500)) {
+ set_team_id(frc971::control_loops::testing::kTeamNumber);
+ set_battery_voltage(12.0);
+
+ if (!FLAGS_output_folder.empty()) {
+ logger_event_loop_ = MakeEventLoop("logger", roborio_);
+ logger_ = std::make_unique<aos::logger::Logger>(logger_event_loop_.get());
+ logger_->StartLoggingOnRun(FLAGS_output_folder);
+ }
+
+ test_event_loop_->OnRun([this]() { SetStartingPosition({3.0, 2.0, 0.0}); });
+
+ imu_test_event_loop_
+ ->AddTimer([this]() {
+ auto builder = localizer_output_sender_.MakeBuilder();
+ frc971::controls::LocalizerOutput::Builder output_builder =
+ builder.MakeBuilder<frc971::controls::LocalizerOutput>();
+ output_builder.add_monotonic_timestamp_ns(
+ imu_test_event_loop_->monotonic_now().time_since_epoch().count());
+ output_builder.add_x(drivetrain_plant_.state()(0));
+ output_builder.add_y(drivetrain_plant_.state()(1));
+ output_builder.add_theta(drivetrain_plant_.state()(2));
+ })
+ ->Setup(imu_test_event_loop_->monotonic_now(),
+ std::chrono::milliseconds(5));
+ }
+
+ virtual ~LocalizedDrivetrainTest() override {}
+
+ void SetStartingPosition(const Eigen::Matrix<double, 3, 1> &xytheta) {
+ *drivetrain_plant_.mutable_state() << xytheta.x(), xytheta.y(),
+ xytheta(2, 0), 0.0, 0.0;
+ Eigen::Matrix<double, Localizer::HybridEkf::kNStates, 1> localizer_state;
+ localizer_state.setZero();
+ localizer_state.block<3, 1>(0, 0) = xytheta;
+ localizer_.Reset(monotonic_now(), localizer_state);
+ }
+
+ void VerifyNearGoal(double eps = 1e-2) {
+ drivetrain_goal_fetcher_.Fetch();
+ EXPECT_NEAR(drivetrain_goal_fetcher_->left_goal(),
+ drivetrain_plant_.GetLeftPosition(), eps);
+ EXPECT_NEAR(drivetrain_goal_fetcher_->right_goal(),
+ drivetrain_plant_.GetRightPosition(), eps);
+ }
+
+ ::testing::AssertionResult IsNear(double expected, double actual,
+ double epsilon) {
+ if (std::abs(expected - actual) < epsilon) {
+ return ::testing::AssertionSuccess();
+ } else {
+ return ::testing::AssertionFailure()
+ << "Expected " << expected << " but got " << actual
+ << " with a max difference of " << epsilon
+ << " and an actual difference of " << std::abs(expected - actual);
+ }
+ }
+ ::testing::AssertionResult VerifyEstimatorAccurate(double eps) {
+ const Eigen::Matrix<double, 5, 1> true_state = drivetrain_plant_.state();
+ ::testing::AssertionResult result(true);
+ if (!(result = IsNear(localizer_.x(), true_state(0), eps))) {
+ return result;
+ }
+ if (!(result = IsNear(localizer_.y(), true_state(1), eps))) {
+ return result;
+ }
+ if (!(result = IsNear(localizer_.theta(), true_state(2), eps))) {
+ return result;
+ }
+ if (!(result = IsNear(localizer_.left_velocity(), true_state(3), eps))) {
+ return result;
+ }
+ if (!(result = IsNear(localizer_.right_velocity(), true_state(4), eps))) {
+ return result;
+ }
+ return result;
+ }
+
+ const aos::Node *const roborio_;
+ const aos::Node *const imu_;
+
+ std::unique_ptr<aos::EventLoop> test_event_loop_;
+ std::unique_ptr<aos::EventLoop> imu_test_event_loop_;
+ aos::Sender<Goal> drivetrain_goal_sender_;
+ aos::Sender<frc971::controls::LocalizerOutput> localizer_output_sender_;
+ aos::Fetcher<Goal> drivetrain_goal_fetcher_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+ aos::Sender<LocalizerControl> localizer_control_sender_;
+
+ std::unique_ptr<aos::EventLoop> drivetrain_event_loop_;
+ const frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+
+ Localizer localizer_;
+ DrivetrainLoop drivetrain_;
+
+ std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
+ std::unique_ptr<aos::EventLoop> drivetrain_plant_imu_event_loop_;
+ DrivetrainSimulation drivetrain_plant_;
+
+ void SendGoal(double left, double right) {
+ auto builder = drivetrain_goal_sender_.MakeBuilder();
+
+ Goal::Builder drivetrain_builder = builder.MakeBuilder<Goal>();
+ drivetrain_builder.add_controller_type(
+ frc971::control_loops::drivetrain::ControllerType::MOTION_PROFILE);
+ drivetrain_builder.add_left_goal(left);
+ drivetrain_builder.add_right_goal(right);
+
+ EXPECT_EQ(builder.Send(drivetrain_builder.Finish()),
+ aos::RawSender::Error::kOk);
+ }
+
+ private:
+ std::unique_ptr<aos::EventLoop> logger_event_loop_;
+ std::unique_ptr<aos::logger::Logger> logger_;
+};
+
+TEST_F(LocalizedDrivetrainTest, Nominal) {
+ SetEnabled(true);
+ EXPECT_TRUE(VerifyEstimatorAccurate(1e-7));
+
+ SendGoal(-1.0, 1.0);
+
+ RunFor(chrono::seconds(10));
+ VerifyNearGoal();
+ EXPECT_TRUE(VerifyEstimatorAccurate(5e-3));
+}
+
+} // namespace testing
+} // namespace drivetrain
+} // namespace control_loops
+} // namespace y2022
diff --git a/y2022/control_loops/localizer/BUILD b/y2022/control_loops/localizer/BUILD
new file mode 100644
index 0000000..034c779
--- /dev/null
+++ b/y2022/control_loops/localizer/BUILD
@@ -0,0 +1,119 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
+load("@npm//@bazel/typescript:index.bzl", "ts_library")
+
+ts_library(
+ name = "localizer_plotter",
+ srcs = ["localizer_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ "//frc971/wpilib:imu_plot_utils",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "localizer_output_fbs",
+ srcs = [
+ "localizer_output.fbs",
+ ],
+ gen_reflections = True,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_cc_library(
+ name = "localizer_status_fbs",
+ srcs = [
+ "localizer_status.fbs",
+ ],
+ gen_reflections = True,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs_includes",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+cc_static_flatbuffer(
+ name = "localizer_schema",
+ function = "frc971::controls::LocalizerStatusSchema",
+ target = ":localizer_status_fbs_reflection_out",
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "localizer",
+ srcs = ["localizer.cc"],
+ hdrs = ["localizer.h"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":localizer_output_fbs",
+ ":localizer_status_fbs",
+ "//aos/containers:ring_buffer",
+ "//aos/events:event_loop",
+ "//aos/time",
+ "//frc971/control_loops:c2d",
+ "//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/control_loops/drivetrain:localizer_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
+ "//frc971/wpilib:imu_fbs",
+ "//frc971/zeroing:imu_zeroer",
+ "//y2020/vision/sift:sift_fbs",
+ "@org_tuxfamily_eigen//:eigen",
+ ],
+)
+
+cc_binary(
+ name = "localizer_main",
+ srcs = ["localizer_main.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":localizer",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//y2022/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
+cc_test(
+ name = "localizer_test",
+ srcs = ["localizer_test.cc"],
+ data = [
+ "//y2022:config",
+ ],
+ shard_count = 10,
+ deps = [
+ ":localizer",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ "//frc971/control_loops/drivetrain:drivetrain_test_lib",
+ ],
+)
+
+cc_binary(
+ name = "localizer_replay",
+ srcs = ["localizer_replay.cc"],
+ data = [
+ "//y2020:config",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":localizer",
+ ":localizer_schema",
+ "//aos:configuration",
+ "//aos:init",
+ "//aos:json_to_flatbuffer",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//aos/events/logging:log_writer",
+ "//y2020/control_loops/drivetrain:drivetrain_base",
+ ],
+)
diff --git a/y2022/control_loops/localizer/localizer.cc b/y2022/control_loops/localizer/localizer.cc
new file mode 100644
index 0000000..3c68e59
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer.cc
@@ -0,0 +1,571 @@
+#include "y2022/control_loops/localizer/localizer.h"
+
+#include "frc971/control_loops/c2d.h"
+#include "frc971/wpilib/imu_batch_generated.h"
+// TODO(james): Things to do:
+// -Approach still seems fundamentally sound.
+// -Really need to work on cost/residual function.
+// -Particularly for handling stopping.
+// -Extra hysteresis
+
+namespace frc971::controls {
+
+namespace {
+constexpr double kG = 9.80665;
+constexpr std::chrono::microseconds kNominalDt(500);
+
+template <int N>
+Eigen::Matrix<double, N, 1> MakeState(std::vector<double> values) {
+ CHECK_EQ(static_cast<size_t>(N), values.size());
+ Eigen::Matrix<double, N, 1> vector;
+ for (int ii = 0; ii < N; ++ii) {
+ vector(ii, 0) = values[ii];
+ }
+ return vector;
+}
+} // namespace
+
+ModelBasedLocalizer::ModelBasedLocalizer(
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
+ : dt_config_(dt_config),
+ velocity_drivetrain_coefficients_(
+ dt_config.make_hybrid_drivetrain_velocity_loop()
+ .plant()
+ .coefficients()),
+ down_estimator_(dt_config) {
+ CHECK_EQ(branches_.capacity(), static_cast<size_t>(std::chrono::seconds(1) /
+ kNominalDt / kBranchPeriod));
+ if (dt_config_.is_simulated) {
+ down_estimator_.assume_perfect_gravity();
+ }
+ A_continuous_accel_.setZero();
+ A_continuous_model_.setZero();
+ B_continuous_accel_.setZero();
+ B_continuous_model_.setZero();
+
+ A_continuous_accel_(kX, kVelocityX) = 1.0;
+ A_continuous_accel_(kY, kVelocityY) = 1.0;
+
+ const double diameter = 2.0 * dt_config_.robot_radius;
+
+ A_continuous_model_(kTheta, kLeftVelocity) = -1.0 / diameter;
+ A_continuous_model_(kTheta, kRightVelocity) = 1.0 / diameter;
+ A_continuous_model_(kLeftEncoder, kLeftVelocity) = 1.0;
+ A_continuous_model_(kRightEncoder, kRightVelocity) = 1.0;
+ const auto &vel_coefs = velocity_drivetrain_coefficients_;
+ A_continuous_model_(kLeftVelocity, kLeftVelocity) =
+ vel_coefs.A_continuous(0, 0);
+ A_continuous_model_(kLeftVelocity, kRightVelocity) =
+ vel_coefs.A_continuous(0, 1);
+ A_continuous_model_(kRightVelocity, kLeftVelocity) =
+ vel_coefs.A_continuous(1, 0);
+ A_continuous_model_(kRightVelocity, kRightVelocity) =
+ vel_coefs.A_continuous(1, 1);
+
+ A_continuous_model_(kLeftVelocity, kLeftVoltageError) =
+ 1 * vel_coefs.B_continuous(0, 0);
+ A_continuous_model_(kLeftVelocity, kRightVoltageError) =
+ 1 * vel_coefs.B_continuous(0, 1);
+ A_continuous_model_(kRightVelocity, kLeftVoltageError) =
+ 1 * vel_coefs.B_continuous(1, 0);
+ A_continuous_model_(kRightVelocity, kRightVoltageError) =
+ 1 * vel_coefs.B_continuous(1, 1);
+
+ B_continuous_model_.block<1, 2>(kLeftVelocity, kLeftVoltage) =
+ vel_coefs.B_continuous.row(0);
+ B_continuous_model_.block<1, 2>(kRightVelocity, kLeftVoltage) =
+ vel_coefs.B_continuous.row(1);
+
+ B_continuous_accel_(kVelocityX, kAccelX) = 1.0;
+ B_continuous_accel_(kVelocityY, kAccelY) = 1.0;
+ B_continuous_accel_(kTheta, kThetaRate) = 1.0;
+
+ Q_continuous_model_.setZero();
+ Q_continuous_model_.diagonal() << 1e-4, 1e-4, 1e-4, 1e-2, 1e-0, 1e-0, 1e-2,
+ 1e-0, 1e-0;
+
+ P_model_ = Q_continuous_model_ * aos::time::DurationInSeconds(kNominalDt);
+}
+
+Eigen::Matrix<double, ModelBasedLocalizer::kNModelStates,
+ ModelBasedLocalizer::kNModelStates>
+ModelBasedLocalizer::AModel(
+ const ModelBasedLocalizer::ModelState &state) const {
+ Eigen::Matrix<double, kNModelStates, kNModelStates> A = A_continuous_model_;
+ const double theta = state(kTheta);
+ const double stheta = std::sin(theta);
+ const double ctheta = std::cos(theta);
+ const double velocity = (state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
+ A(kX, kTheta) = -stheta * velocity;
+ A(kX, kLeftVelocity) = ctheta / 2.0;
+ A(kX, kRightVelocity) = ctheta / 2.0;
+ A(kY, kTheta) = ctheta * velocity;
+ A(kY, kLeftVelocity) = stheta / 2.0;
+ A(kY, kRightVelocity) = stheta / 2.0;
+ return A;
+}
+
+Eigen::Matrix<double, ModelBasedLocalizer::kNAccelStates,
+ ModelBasedLocalizer::kNAccelStates>
+ModelBasedLocalizer::AAccel() const {
+ return A_continuous_accel_;
+}
+
+ModelBasedLocalizer::ModelState ModelBasedLocalizer::DiffModel(
+ const ModelBasedLocalizer::ModelState &state,
+ const ModelBasedLocalizer::ModelInput &U) const {
+ ModelState x_dot = AModel(state) * state + B_continuous_model_ * U;
+ const double theta = state(kTheta);
+ const double stheta = std::sin(theta);
+ const double ctheta = std::cos(theta);
+ const double velocity = (state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
+ x_dot(kX) = ctheta * velocity;
+ x_dot(kY) = stheta * velocity;
+ return x_dot;
+}
+
+ModelBasedLocalizer::AccelState ModelBasedLocalizer::DiffAccel(
+ const ModelBasedLocalizer::AccelState &state,
+ const ModelBasedLocalizer::AccelInput &U) const {
+ return AAccel() * state + B_continuous_accel_ * U;
+}
+
+ModelBasedLocalizer::ModelState ModelBasedLocalizer::UpdateModel(
+ const ModelBasedLocalizer::ModelState &model,
+ const ModelBasedLocalizer::ModelInput &input,
+ const aos::monotonic_clock::duration dt) const {
+ return control_loops::RungeKutta(
+ std::bind(&ModelBasedLocalizer::DiffModel, this, std::placeholders::_1,
+ input),
+ model, aos::time::DurationInSeconds(dt));
+}
+
+ModelBasedLocalizer::AccelState ModelBasedLocalizer::UpdateAccel(
+ const ModelBasedLocalizer::AccelState &accel,
+ const ModelBasedLocalizer::AccelInput &input,
+ const aos::monotonic_clock::duration dt) const {
+ return control_loops::RungeKutta(
+ std::bind(&ModelBasedLocalizer::DiffAccel, this, std::placeholders::_1,
+ input),
+ accel, aos::time::DurationInSeconds(dt));
+}
+
+ModelBasedLocalizer::AccelState ModelBasedLocalizer::AccelStateForModelState(
+ const ModelBasedLocalizer::ModelState &state) const {
+ const double robot_speed =
+ (state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
+ const double velocity_x = std::cos(state(kTheta)) * robot_speed;
+ const double velocity_y = std::sin(state(kTheta)) * robot_speed;
+ return (AccelState() << state(0), state(1), state(2), velocity_x, velocity_y)
+ .finished();
+}
+
+ModelBasedLocalizer::ModelState ModelBasedLocalizer::ModelStateForAccelState(
+ const ModelBasedLocalizer::AccelState &state,
+ const Eigen::Vector2d &encoders, const double yaw_rate) const {
+ const double robot_speed = state(kVelocityX) * std::cos(state(kTheta)) +
+ state(kVelocityY) * std::sin(state(kTheta));
+ const double radius = dt_config_.robot_radius;
+ const double left_velocity = robot_speed - yaw_rate * radius;
+ const double right_velocity = robot_speed + yaw_rate * radius;
+ return (ModelState() << state(0), state(1), state(2), encoders(0),
+ left_velocity, 0.0, encoders(1), right_velocity, 0.0)
+ .finished();
+}
+
+double ModelBasedLocalizer::ModelDivergence(
+ const ModelBasedLocalizer::CombinedState &state,
+ const ModelBasedLocalizer::AccelInput &accel_inputs,
+ const Eigen::Vector2d &filtered_accel,
+ const ModelBasedLocalizer::ModelInput &model_inputs) {
+ // Convert the model state into the acceleration-based state-space and check
+ // the distance between the two (should really be a weighted norm, but all the
+ // numbers are on ~the same scale).
+ VLOG(2) << "divergence: "
+ << (state.accel_state - AccelStateForModelState(state.model_state))
+ .transpose();
+ const AccelState diff_accel = DiffAccel(state.accel_state, accel_inputs);
+ const ModelState diff_model = DiffModel(state.model_state, model_inputs);
+ const double model_lng_velocity =
+ (state.model_state(kLeftVelocity) + state.model_state(kRightVelocity)) /
+ 2.0;
+ const double model_lng_accel =
+ (diff_model(kLeftVelocity) + diff_model(kRightVelocity)) / 2.0;
+ const Eigen::Vector2d robot_frame_accel(
+ model_lng_accel, diff_model(kTheta) * model_lng_velocity);
+ const Eigen::Vector2d model_accel =
+ Eigen::AngleAxisd(state.model_state(kTheta), Eigen::Vector3d::UnitZ())
+ .toRotationMatrix()
+ .block<2, 2>(0, 0) *
+ robot_frame_accel;
+ const double accel_diff = (model_accel - filtered_accel).norm();
+ const double theta_rate_diff =
+ std::abs(diff_accel(kTheta) - diff_model(kTheta));
+
+ const Eigen::Vector2d accel_vel = state.accel_state.bottomRows<2>();
+ const Eigen::Vector2d model_vel =
+ AccelStateForModelState(state.model_state).bottomRows<2>();
+ velocity_residual_ = (accel_vel - model_vel).norm() /
+ (1.0 + accel_vel.norm() + model_vel.norm());
+ theta_rate_residual_ = theta_rate_diff;
+ accel_residual_ = accel_diff / 4.0;
+ return velocity_residual_ + theta_rate_residual_ + accel_residual_;
+}
+
+void ModelBasedLocalizer::HandleImu(aos::monotonic_clock::time_point t,
+ const Eigen::Vector3d &gyro,
+ const Eigen::Vector3d &accel,
+ const Eigen::Vector2d encoders,
+ const Eigen::Vector2d voltage) {
+ VLOG(2) << t;
+ if (t_ == aos::monotonic_clock::min_time) {
+ t_ = t;
+ }
+ if (t_ + 2 * kNominalDt < t) {
+ t_ = t;
+ ++clock_resets_;
+ }
+ const aos::monotonic_clock::duration dt = t - t_;
+ t_ = t;
+ down_estimator_.Predict(gyro, accel, dt);
+ // TODO(james): Should we prefer this or use the down-estimator corrected
+ // version?
+ const double yaw_rate = (dt_config_.imu_transform * gyro)(2);
+ const double diameter = 2.0 * dt_config_.robot_radius;
+
+ const Eigen::AngleAxis<double> orientation(
+ Eigen::AngleAxis<double>(xytheta()(kTheta), Eigen::Vector3d::UnitZ()) *
+ down_estimator_.X_hat());
+
+ const Eigen::Vector3d absolute_accel =
+ orientation * dt_config_.imu_transform * kG * accel;
+ abs_accel_ = absolute_accel;
+ const Eigen::Vector3d absolute_gyro =
+ orientation * dt_config_.imu_transform * gyro;
+ (void)absolute_gyro;
+
+ VLOG(2) << "abs accel " << absolute_accel.transpose();
+ VLOG(2) << "abs gyro " << absolute_gyro.transpose();
+ VLOG(2) << "dt " << aos::time::DurationInSeconds(dt);
+
+ // Update all the branched states.
+ const AccelInput accel_input(absolute_accel.x(), absolute_accel.y(),
+ yaw_rate);
+ const ModelInput model_input(voltage);
+
+ const Eigen::Matrix<double, kNModelStates, kNModelStates> A_continuous =
+ AModel(current_state_.model_state);
+
+ Eigen::Matrix<double, kNModelStates, kNModelStates> A_discrete;
+ Eigen::Matrix<double, kNModelStates, kNModelStates> Q_discrete;
+
+ DiscretizeQAFast(Q_continuous_model_, A_continuous, dt, &Q_discrete,
+ &A_discrete);
+
+ P_model_ = A_discrete * P_model_ * A_discrete.transpose() + Q_discrete;
+
+ Eigen::Matrix<double, kNModelOutputs, kNModelStates> H;
+ Eigen::Matrix<double, kNModelOutputs, kNModelOutputs> R;
+ {
+ H.setZero();
+ R.setZero();
+ H(0, kLeftEncoder) = 1.0;
+ H(1, kRightEncoder) = 1.0;
+ H(2, kRightVelocity) = 1.0 / diameter;
+ H(2, kLeftVelocity) = -1.0 / diameter;
+
+ R.diagonal() << 1e-9, 1e-9, 1e-13;
+ }
+
+ const Eigen::Matrix<double, kNModelOutputs, 1> Z(encoders(0), encoders(1),
+ yaw_rate);
+
+ if (branches_.empty()) {
+ VLOG(2) << "Initializing";
+ current_state_.model_state.setZero();
+ current_state_.accel_state.setZero();
+ current_state_.model_state(kLeftEncoder) = encoders(0);
+ current_state_.model_state(kRightEncoder) = encoders(1);
+ current_state_.branch_time = t;
+ branches_.Push(current_state_);
+ }
+
+ const Eigen::Matrix<double, kNModelStates, kNModelOutputs> K =
+ P_model_ * H.transpose() * (H * P_model_ * H.transpose() + R).inverse();
+ P_model_ = (Eigen::Matrix<double, kNModelStates, kNModelStates>::Identity() -
+ K * H) *
+ P_model_;
+ VLOG(2) << "K\n" << K;
+ VLOG(2) << "Z\n" << Z.transpose();
+
+ for (CombinedState &state : branches_) {
+ state.accel_state = UpdateAccel(state.accel_state, accel_input, dt);
+ if (down_estimator_.consecutive_still() > 100.0) {
+ state.accel_state(kVelocityX) *= 0.9;
+ state.accel_state(kVelocityY) *= 0.9;
+ }
+ state.model_state = UpdateModel(state.model_state, model_input, dt);
+ state.model_state += K * (Z - H * state.model_state);
+ }
+
+ VLOG(2) << "oildest accel " << branches_[0].accel_state.transpose();
+ VLOG(2) << "oildest accel diff "
+ << DiffAccel(branches_[0].accel_state, accel_input).transpose();
+ VLOG(2) << "oildest model " << branches_[0].model_state.transpose();
+
+ // Determine whether to switch modes--if we are currently in model-based mode,
+ // swap to accel-based if the two states have divergeed meaningfully in the
+ // oldest branch. If we are currently in accel-based, then swap back to model
+ // if the oldest model branch matches has matched the
+ filtered_residual_accel_ +=
+ 0.01 * (accel_input.topRows<2>() - filtered_residual_accel_);
+ const double model_divergence =
+ branches_.full() ? ModelDivergence(branches_[0], accel_input,
+ filtered_residual_accel_, model_input)
+ : 0.0;
+ filtered_residual_ +=
+ (1.0 - std::exp(-aos::time::DurationInSeconds(kNominalDt) / 0.0095)) *
+ (model_divergence - filtered_residual_);
+ constexpr double kUseAccelThreshold = 0.4;
+ constexpr double kUseModelThreshold = 0.2;
+ constexpr size_t kShareStates = kNModelStates;
+ static_assert(kUseModelThreshold < kUseAccelThreshold);
+ if (using_model_) {
+ if (filtered_residual_ > kUseAccelThreshold) {
+ hysteresis_count_++;
+ } else {
+ hysteresis_count_ = 0;
+ }
+ if (hysteresis_count_ > 0) {
+ using_model_ = false;
+ // Grab the accel-based state from back when we started diverging.
+ // TODO(james): This creates a problematic selection bias, because
+ // we will tend to bias towards deliberately out-of-tune measurements.
+ current_state_.accel_state = branches_[0].accel_state;
+ current_state_.model_state = branches_[0].model_state;
+ current_state_.model_state = ModelStateForAccelState(
+ current_state_.accel_state, encoders, yaw_rate);
+ } else {
+ VLOG(2) << "Normal branching";
+ current_state_.model_state = branches_[branches_.size() - 1].model_state;
+ current_state_.accel_state =
+ AccelStateForModelState(current_state_.model_state);
+ current_state_.branch_time = t;
+ }
+ hysteresis_count_ = 0;
+ } else {
+ if (filtered_residual_ < kUseModelThreshold) {
+ hysteresis_count_++;
+ } else {
+ hysteresis_count_ = 0;
+ }
+ if (hysteresis_count_ > 100) {
+ using_model_ = true;
+ // Grab the model-based state from back when we stopped diverging.
+ current_state_.model_state.topRows<kShareStates>() =
+ ModelStateForAccelState(branches_[0].accel_state, encoders, yaw_rate)
+ .topRows<kShareStates>();
+ current_state_.accel_state =
+ AccelStateForModelState(current_state_.model_state);
+ } else {
+ current_state_.accel_state = branches_[branches_.size() - 1].accel_state;
+ // TODO(james): Why was I leaving the encoders/wheel velocities in place?
+ current_state_.model_state = branches_[branches_.size() - 1].model_state;
+ current_state_.model_state = ModelStateForAccelState(
+ current_state_.accel_state, encoders, yaw_rate);
+ current_state_.branch_time = t;
+ }
+ }
+
+ // Generate a new branch, with the accel state reset based on the model-based
+ // state (really, just getting rid of the lateral velocity).
+ CombinedState new_branch = current_state_;
+ //if (!keep_accel_state) {
+ if (using_model_) {
+ new_branch.accel_state = AccelStateForModelState(new_branch.model_state);
+ }
+ new_branch.accumulated_divergence = 0.0;
+
+ ++branch_counter_;
+ if (branch_counter_ % kBranchPeriod == 0) {
+ branches_.Push(new_branch);
+ branch_counter_ = 0;
+ }
+
+ last_residual_ = model_divergence;
+
+ VLOG(2) << "Using " << (using_model_ ? "model" : "accel");
+ VLOG(2) << "Residual " << last_residual_;
+ VLOG(2) << "Filtered Residual " << filtered_residual_;
+ VLOG(2) << "buffer size " << branches_.size();
+ VLOG(2) << "Model state " << current_state_.model_state.transpose();
+ VLOG(2) << "Accel state " << current_state_.accel_state.transpose();
+ VLOG(2) << "Accel state for model "
+ << AccelStateForModelState(current_state_.model_state).transpose();
+ VLOG(2) << "Input acce " << accel.transpose();
+ VLOG(2) << "Input gyro " << gyro.transpose();
+ VLOG(2) << "Input voltage " << voltage.transpose();
+ VLOG(2) << "Input encoder " << encoders.transpose();
+ VLOG(2) << "yaw rate " << yaw_rate;
+
+ CHECK(std::isfinite(last_residual_));
+}
+
+void ModelBasedLocalizer::HandleReset(aos::monotonic_clock::time_point now,
+ const Eigen::Vector3d &xytheta) {
+ branches_.Reset();
+ t_ = now;
+ using_model_ = true;
+ current_state_.model_state << xytheta(0), xytheta(1), xytheta(2),
+ current_state_.model_state(kLeftEncoder), 0.0, 0.0,
+ current_state_.model_state(kRightEncoder), 0.0, 0.0;
+ current_state_.accel_state =
+ AccelStateForModelState(current_state_.model_state);
+ last_residual_ = 0.0;
+ filtered_residual_ = 0.0;
+ filtered_residual_accel_.setZero();
+ abs_accel_.setZero();
+}
+
+flatbuffers::Offset<AccelBasedState> ModelBasedLocalizer::BuildAccelState(
+ flatbuffers::FlatBufferBuilder *fbb, const AccelState &state) {
+ AccelBasedState::Builder accel_state_builder(*fbb);
+ accel_state_builder.add_x(state(kX));
+ accel_state_builder.add_y(state(kY));
+ accel_state_builder.add_theta(state(kTheta));
+ accel_state_builder.add_velocity_x(state(kVelocityX));
+ accel_state_builder.add_velocity_y(state(kVelocityY));
+ return accel_state_builder.Finish();
+}
+
+flatbuffers::Offset<ModelBasedState> ModelBasedLocalizer::BuildModelState(
+ flatbuffers::FlatBufferBuilder *fbb, const ModelState &state) {
+ ModelBasedState::Builder model_state_builder(*fbb);
+ model_state_builder.add_x(state(kX));
+ model_state_builder.add_y(state(kY));
+ model_state_builder.add_theta(state(kTheta));
+ model_state_builder.add_left_encoder(state(kLeftEncoder));
+ model_state_builder.add_left_velocity(state(kLeftVelocity));
+ model_state_builder.add_left_voltage_error(state(kLeftVoltageError));
+ model_state_builder.add_right_encoder(state(kRightEncoder));
+ model_state_builder.add_right_velocity(state(kRightVelocity));
+ model_state_builder.add_right_voltage_error(state(kRightVoltageError));
+ return model_state_builder.Finish();
+}
+
+flatbuffers::Offset<ModelBasedStatus> ModelBasedLocalizer::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) {
+ const flatbuffers::Offset<control_loops::drivetrain::DownEstimatorState>
+ down_estimator_offset = down_estimator_.PopulateStatus(fbb, t_);
+
+ const CombinedState &state = current_state_;
+
+ const flatbuffers::Offset<ModelBasedState> model_state_offset =
+ BuildModelState(fbb, state.model_state);
+
+ const flatbuffers::Offset<AccelBasedState> accel_state_offset =
+ BuildAccelState(fbb, state.accel_state);
+
+ const flatbuffers::Offset<AccelBasedState> oldest_accel_state_offset =
+ branches_.empty() ? flatbuffers::Offset<AccelBasedState>()
+ : BuildAccelState(fbb, branches_[0].accel_state);
+
+ const flatbuffers::Offset<ModelBasedState> oldest_model_state_offset =
+ branches_.empty() ? flatbuffers::Offset<ModelBasedState>()
+ : BuildModelState(fbb, branches_[0].model_state);
+
+ ModelBasedStatus::Builder builder(*fbb);
+ builder.add_accel_state(accel_state_offset);
+ builder.add_oldest_accel_state(oldest_accel_state_offset);
+ builder.add_oldest_model_state(oldest_model_state_offset);
+ builder.add_model_state(model_state_offset);
+ builder.add_using_model(using_model_);
+ builder.add_residual(last_residual_);
+ builder.add_filtered_residual(filtered_residual_);
+ builder.add_velocity_residual(velocity_residual_);
+ builder.add_accel_residual(accel_residual_);
+ builder.add_theta_rate_residual(theta_rate_residual_);
+ builder.add_down_estimator(down_estimator_offset);
+ builder.add_x(xytheta()(0));
+ builder.add_y(xytheta()(1));
+ builder.add_theta(xytheta()(2));
+ builder.add_implied_accel_x(abs_accel_(0));
+ builder.add_implied_accel_y(abs_accel_(1));
+ builder.add_implied_accel_z(abs_accel_(2));
+ builder.add_clock_resets(clock_resets_);
+ return builder.Finish();
+}
+
+EventLoopLocalizer::EventLoopLocalizer(
+ aos::EventLoop *event_loop,
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
+ : event_loop_(event_loop),
+ model_based_(dt_config),
+ status_sender_(event_loop_->MakeSender<LocalizerStatus>("/localizer")),
+ output_sender_(event_loop_->MakeSender<LocalizerOutput>("/localizer")),
+ output_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
+ "/drivetrain")) {
+ event_loop_->MakeWatcher(
+ "/drivetrain",
+ [this](
+ const frc971::control_loops::drivetrain::LocalizerControl &control) {
+ const double theta = control.keep_current_theta()
+ ? model_based_.xytheta()(2)
+ : control.theta();
+ model_based_.HandleReset(event_loop_->monotonic_now(),
+ {control.x(), control.y(), theta});
+ });
+ event_loop_->MakeWatcher(
+ "/localizer", [this](const frc971::IMUValuesBatch &values) {
+ CHECK(values.has_readings());
+ output_fetcher_.Fetch();
+ for (const IMUValues *value : *values.readings()) {
+ zeroer_.InsertAndProcessMeasurement(*value);
+ if (zeroer_.Zeroed()) {
+ if (output_fetcher_.get() != nullptr) {
+ const bool disabled =
+ output_fetcher_.context().monotonic_event_time +
+ std::chrono::milliseconds(10) <
+ event_loop_->context().monotonic_event_time;
+ model_based_.HandleImu(
+ aos::monotonic_clock::time_point(std::chrono::nanoseconds(
+ value->monotonic_timestamp_ns())),
+ zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(),
+ {value->left_encoder(), value->right_encoder()},
+ disabled ? Eigen::Vector2d::Zero()
+ : Eigen::Vector2d{output_fetcher_->left_voltage(),
+ output_fetcher_->right_voltage()});
+ }
+ }
+ {
+ auto builder = status_sender_.MakeBuilder();
+ const flatbuffers::Offset<ModelBasedStatus> model_based_status =
+ model_based_.PopulateStatus(builder.fbb());
+ LocalizerStatus::Builder status_builder =
+ builder.MakeBuilder<LocalizerStatus>();
+ status_builder.add_model_based(model_based_status);
+ status_builder.add_zeroed(zeroer_.Zeroed());
+ status_builder.add_faulted_zero(zeroer_.Faulted());
+ builder.CheckOk(builder.Send(status_builder.Finish()));
+ }
+ if (last_output_send_ + std::chrono::milliseconds(5) <
+ event_loop_->context().monotonic_event_time) {
+ auto builder = output_sender_.MakeBuilder();
+ LocalizerOutput::Builder output_builder =
+ builder.MakeBuilder<LocalizerOutput>();
+ // TODO(james): Should we bother to try to estimate time offsets for
+ // the pico?
+ output_builder.add_monotonic_timestamp_ns(
+ value->monotonic_timestamp_ns());
+ output_builder.add_x(model_based_.xytheta()(0));
+ output_builder.add_y(model_based_.xytheta()(1));
+ output_builder.add_theta(model_based_.xytheta()(2));
+ builder.CheckOk(builder.Send(output_builder.Finish()));
+ last_output_send_ = event_loop_->monotonic_now();
+ }
+ }
+ });
+}
+
+} // namespace frc971::controls
diff --git a/y2022/control_loops/localizer/localizer.h b/y2022/control_loops/localizer/localizer.h
new file mode 100644
index 0000000..bb52a40
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer.h
@@ -0,0 +1,220 @@
+#ifndef Y2022_CONTROL_LOOPS_LOCALIZER_LOCALIZER_H_
+#define Y2022_CONTROL_LOOPS_LOCALIZER_LOCALIZER_H_
+
+#include "Eigen/Dense"
+#include "Eigen/Geometry"
+
+#include "aos/events/event_loop.h"
+#include "aos/containers/ring_buffer.h"
+#include "aos/time/time.h"
+#include "y2020/vision/sift/sift_generated.h"
+#include "y2022/control_loops/localizer/localizer_status_generated.h"
+#include "y2022/control_loops/localizer/localizer_output_generated.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "frc971/zeroing/imu_zeroer.h"
+
+namespace frc971::controls {
+
+namespace testing {
+class LocalizerTest;
+}
+
+// Localizer implementation that makes use of a 6-axis IMU, encoder readings,
+// drivetrain voltages, and camera returns to localize the robot. Meant to
+// be run on a raspberry pi.
+//
+// This operates on the principle that the drivetrain can be in one of two
+// modes:
+// 1) A "normal" mode where it is obeying the regular drivetrain model, with
+// minimal lateral motion and no major external disturbances. This is
+// referred to as the "model" mode in the code/variable names.
+// 2) An non-holonomic mode where the robot is just flying around on a 2-D
+// plane with no meaningful constraints (referred to as an "accel" model
+// in the code, because we rely primarily on the accelerometer readings).
+//
+// In order to determine which mode to be in, we attempt to track whether the
+// two models are diverging substantially. In order to do this, we maintain a
+// 1-second long queue of "branches". A branch is generated every X iterations
+// and contains a model state and an accel state. When the branch starts, the
+// two will have identical states. For the remaining 1 second, the model state
+// will evolve purely according to the drivetrian model, and the accel state
+// will evolve purely using IMU readings.
+//
+// When the branch reaches 1 second in age, we calculate a residual associated
+// with how much the accel and model based states diverged. If they have
+// diverged substantially, that implies that the model is a poor match for
+// whatever has been happening to the robot in the past second, so if we were
+// previously relying on the model, we will override the current "actual"
+// state with the branched accel state, and then continue to update the accel
+// state based on IMU readings.
+// If we are currently in the accel state, we will continue generating branches
+// until the branches stop diverging--this will indicate that the model
+// matches the accelerometer readings again, and so we will swap back to
+// the model-based state.
+//
+// TODO:
+// * Implement paying attention to camera readings.
+// * Tune for ADIS16505/real robot.
+// * Tune down CPU usage to run on a pi.
+class ModelBasedLocalizer {
+ public:
+ static constexpr size_t kX = 0;
+ static constexpr size_t kY = 1;
+ static constexpr size_t kTheta = 2;
+
+ static constexpr size_t kVelocityX = 3;
+ static constexpr size_t kVelocityY = 4;
+ static constexpr size_t kNAccelStates = 5;
+
+ static constexpr size_t kLeftEncoder = 3;
+ static constexpr size_t kLeftVelocity = 4;
+ static constexpr size_t kLeftVoltageError = 5;
+ static constexpr size_t kRightEncoder = 6;
+ static constexpr size_t kRightVelocity = 7;
+ static constexpr size_t kRightVoltageError = 8;
+ static constexpr size_t kNModelStates = 9;
+
+ static constexpr size_t kNModelOutputs = 3;
+
+ static constexpr size_t kNAccelOuputs = 1;
+
+ static constexpr size_t kAccelX = 0;
+ static constexpr size_t kAccelY = 1;
+ static constexpr size_t kThetaRate = 2;
+ static constexpr size_t kNAccelInputs = 3;
+
+ static constexpr size_t kLeftVoltage = 0;
+ static constexpr size_t kRightVoltage = 1;
+ static constexpr size_t kNModelInputs = 2;
+
+ // Branching period, in cycles.
+ static constexpr int kBranchPeriod = 1;
+
+ typedef Eigen::Matrix<double, kNModelStates, 1> ModelState;
+ typedef Eigen::Matrix<double, kNAccelStates, 1> AccelState;
+ typedef Eigen::Matrix<double, kNModelInputs, 1> ModelInput;
+ typedef Eigen::Matrix<double, kNAccelInputs, 1> AccelInput;
+
+ ModelBasedLocalizer(
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
+ void HandleImu(aos::monotonic_clock::time_point t,
+ const Eigen::Vector3d &gyro, const Eigen::Vector3d &accel,
+ const Eigen::Vector2d encoders, const Eigen::Vector2d voltage);
+ void HandleImageMatch(aos::monotonic_clock::time_point,
+ const vision::sift::ImageMatchResult *) {
+ LOG(FATAL) << "Unimplemented.";
+ }
+ void HandleReset(aos::monotonic_clock::time_point,
+ const Eigen::Vector3d &xytheta);
+
+ flatbuffers::Offset<ModelBasedStatus> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb);
+
+ Eigen::Vector3d xytheta() const {
+ if (using_model_) {
+ return current_state_.model_state.block<3, 1>(0, 0);
+ } else {
+ return current_state_.accel_state.block<3, 1>(0, 0);
+ }
+ }
+
+ AccelState accel_state() const { return current_state_.accel_state; };
+
+ private:
+ struct CombinedState {
+ AccelState accel_state;
+ ModelState model_state;
+ aos::monotonic_clock::time_point branch_time;
+ double accumulated_divergence;
+ };
+
+ static flatbuffers::Offset<AccelBasedState> BuildAccelState(
+ flatbuffers::FlatBufferBuilder *fbb, const AccelState &state);
+
+ static flatbuffers::Offset<ModelBasedState> BuildModelState(
+ flatbuffers::FlatBufferBuilder *fbb, const ModelState &state);
+
+ Eigen::Matrix<double, kNModelStates, kNModelStates> AModel(
+ const ModelState &state) const;
+ Eigen::Matrix<double, kNAccelStates, kNAccelStates> AAccel() const;
+ ModelState DiffModel(const ModelState &state, const ModelInput &U) const;
+ AccelState DiffAccel(const AccelState &state, const AccelInput &U) const;
+
+ ModelState UpdateModel(const ModelState &model, const ModelInput &input,
+ aos::monotonic_clock::duration dt) const;
+ AccelState UpdateAccel(const AccelState &accel, const AccelInput &input,
+ aos::monotonic_clock::duration dt) const;
+
+ AccelState AccelStateForModelState(const ModelState &state) const;
+ ModelState ModelStateForAccelState(const AccelState &state,
+ const Eigen::Vector2d &encoders,
+ const double yaw_rate) const;
+ double ModelDivergence(const CombinedState &state,
+ const AccelInput &accel_inputs,
+ const Eigen::Vector2d &filtered_accel,
+ const ModelInput &model_inputs);
+
+ const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ const StateFeedbackHybridPlantCoefficients<2, 2, 2, double>
+ velocity_drivetrain_coefficients_;
+ Eigen::Matrix<double, kNModelStates, kNModelStates> A_continuous_model_;
+ Eigen::Matrix<double, kNAccelStates, kNAccelStates> A_continuous_accel_;
+ Eigen::Matrix<double, kNModelStates, kNModelInputs> B_continuous_model_;
+ Eigen::Matrix<double, kNAccelStates, kNAccelInputs> B_continuous_accel_;
+
+ Eigen::Matrix<double, kNModelStates, kNModelStates> Q_continuous_model_;
+
+ Eigen::Matrix<double, kNModelStates, kNModelStates> P_model_;
+ // When we are following the model, we will, on each iteration:
+ // 1) Perform a model-based update of a single state.
+ // 2) Add a hypothetical non-model-based entry based on the current state.
+ // 3) Evict too-old non-model-based entries.
+ control_loops::drivetrain::DrivetrainUkf down_estimator_;
+
+ // Buffer of old branches these are all created by initializing a new
+ // model-based state based on the current state, and then initializing a new
+ // accel-based state on top of that new model-based state (to eliminate the
+ // impact of any lateral motion).
+ // We then integrate up all of these states and observe how much the model and
+ // accel based states of each branch compare to one another.
+ aos::RingBuffer<CombinedState, 2000 / kBranchPeriod> branches_;
+ int branch_counter_ = 0;
+
+ CombinedState current_state_;
+ aos::monotonic_clock::time_point t_ = aos::monotonic_clock::min_time;
+ bool using_model_;
+
+ double last_residual_ = 0.0;
+ double filtered_residual_ = 0.0;
+ Eigen::Vector2d filtered_residual_accel_ = Eigen::Vector2d::Zero();
+ Eigen::Vector3d abs_accel_ = Eigen::Vector3d::Zero();
+ double velocity_residual_ = 0.0;
+ double accel_residual_ = 0.0;
+ double theta_rate_residual_ = 0.0;
+ int hysteresis_count_ = 0;
+
+ int clock_resets_ = 0;
+
+ friend class testing::LocalizerTest;
+};
+
+class EventLoopLocalizer {
+ public:
+ EventLoopLocalizer(
+ aos::EventLoop *event_loop,
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config);
+
+ private:
+ aos::EventLoop *event_loop_;
+ ModelBasedLocalizer model_based_;
+ aos::Sender<LocalizerStatus> status_sender_;
+ aos::Sender<LocalizerOutput> output_sender_;
+ aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
+ zeroing::ImuZeroer zeroer_;
+ aos::monotonic_clock::time_point last_output_send_ =
+ aos::monotonic_clock::min_time;
+};
+} // namespace frc971::controls
+#endif // Y2022_CONTROL_LOOPS_LOCALIZER_LOCALIZER_H_
diff --git a/y2022/control_loops/localizer/localizer_main.cc b/y2022/control_loops/localizer/localizer_main.cc
new file mode 100644
index 0000000..0cc53bb
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer_main.cc
@@ -0,0 +1,21 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2022/control_loops/localizer/localizer.h"
+#include "y2022/control_loops/drivetrain/drivetrain_base.h"
+
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+ frc971::controls::EventLoopLocalizer localizer(
+ &event_loop, ::y2022::control_loops::drivetrain::GetDrivetrainConfig());
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2022/control_loops/localizer/localizer_output.fbs b/y2022/control_loops/localizer/localizer_output.fbs
new file mode 100644
index 0000000..078d723
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer_output.fbs
@@ -0,0 +1,17 @@
+namespace frc971.controls;
+
+// This provides a minimal output from the localizer that can be forwarded to
+// the roborio and used for corrections to its (simpler) localizer.
+
+table LocalizerOutput {
+ // Timestamp (on the source node) that this sample corresponds with. This
+ // may be older than the sent time to account for delays in sensor readings.
+ monotonic_timestamp_ns:int64 (id: 0);
+ // Current x/y position estimate, in meters.
+ x:double (id: 1);
+ y:double (id: 2);
+ // Current heading, in radians.
+ theta:double (id: 3);
+}
+
+root_type LocalizerOutput;
diff --git a/y2022/control_loops/localizer/localizer_plotter.ts b/y2022/control_loops/localizer/localizer_plotter.ts
new file mode 100644
index 0000000..ce348e7
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer_plotter.ts
@@ -0,0 +1,267 @@
+// Provides a plot for debugging drivetrain-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import {ImuMessageHandler} from 'org_frc971/frc971/wpilib/imu_plot_utils';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+
+export function plotLocalizer(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+
+ const position = aosPlotter.addMessageSource("/drivetrain",
+ "frc971.control_loops.drivetrain.Position");
+ const status = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+ const output = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Output');
+ const localizer = aosPlotter.addMessageSource(
+ '/localizer', 'frc971.controls.LocalizerStatus');
+ const imu = aosPlotter.addRawMessageSource(
+ '/drivetrain', 'frc971.IMUValuesBatch',
+ new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
+
+ // Drivetrain Status estimated relative position
+ const positionPlot = aosPlotter.addPlot(element);
+ positionPlot.plot.getAxisLabels().setTitle("Estimated Relative Position " +
+ "of the Drivetrain");
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
+ positionPlot.plot.getAxisLabels().setYLabel("Relative Position (m)");
+ const leftPosition =
+ positionPlot.addMessageLine(status, ["estimated_left_position"]);
+ leftPosition.setColor(RED);
+ const rightPosition =
+ positionPlot.addMessageLine(status, ["estimated_right_position"]);
+ rightPosition.setColor(GREEN);
+ positionPlot
+ .addMessageLine(localizer, ['model_based', 'model_state', 'left_encoder'])
+ .setColor(BROWN)
+ .setPointSize(0.0);
+ positionPlot
+ .addMessageLine(localizer, ['model_based', 'model_state', 'right_encoder'])
+ .setColor(CYAN)
+ .setPointSize(0.0);
+ positionPlot.addMessageLine(position, ['left_encoder'])
+ .setColor(BROWN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(position, ['right_encoder'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+
+
+ // Drivetrain Velocities
+ const velocityPlot = aosPlotter.addPlot(element);
+ velocityPlot.plot.getAxisLabels().setTitle('Velocity Plots');
+ velocityPlot.plot.getAxisLabels().setXLabel(TIME);
+ velocityPlot.plot.getAxisLabels().setYLabel('Wheel Velocity (m/s)');
+
+ const leftVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_left_velocity']);
+ leftVelocity.setColor(RED);
+ const rightVelocity =
+ velocityPlot.addMessageLine(status, ['estimated_right_velocity']);
+ rightVelocity.setColor(GREEN);
+
+ const leftSpeed = velocityPlot.addMessageLine(position, ["left_speed"]);
+ leftSpeed.setColor(BLUE);
+ const rightSpeed = velocityPlot.addMessageLine(position, ["right_speed"]);
+ rightSpeed.setColor(BROWN);
+
+ const ekfLeftVelocity = velocityPlot.addMessageLine(
+ localizer, ['model_based', 'model_state', 'left_velocity']);
+ ekfLeftVelocity.setColor(RED);
+ ekfLeftVelocity.setPointSize(0.0);
+ const ekfRightVelocity = velocityPlot.addMessageLine(
+ localizer, ['model_based', 'model_state', 'right_velocity']);
+ ekfRightVelocity.setColor(GREEN);
+ ekfRightVelocity.setPointSize(0.0);
+
+ velocityPlot
+ .addMessageLine(
+ localizer, ['model_based', 'oldest_model_state', 'left_velocity'])
+ .setColor(RED)
+ .setDrawLine(false);
+
+ velocityPlot
+ .addMessageLine(
+ localizer, ['model_based', 'oldest_model_state', 'right_velocity'])
+ .setColor(GREEN)
+ .setDrawLine(false);
+
+ const splineVelocityOffset =
+ velocityPlot
+ .addMessageLine(status, ['localizer', 'longitudinal_velocity_offset'])
+ .setColor(BROWN)
+ .setPointSize(0.0);
+
+ const splineLateralVelocity =
+ velocityPlot.addMessageLine(status, ['localizer', 'lateral_velocity'])
+ .setColor(PINK)
+ .setPointSize(0.0);
+
+ // Drivetrain Voltage
+ const voltagePlot = aosPlotter.addPlot(element);
+ voltagePlot.plot.getAxisLabels().setTitle('Voltage Plots');
+ voltagePlot.plot.getAxisLabels().setXLabel(TIME);
+ voltagePlot.plot.getAxisLabels().setYLabel('Voltage (V)')
+
+ voltagePlot.addMessageLine(localizer, ['model_based', 'model_state', 'left_voltage_error'])
+ .setColor(RED)
+ .setDrawLine(false);
+ voltagePlot.addMessageLine(localizer, ['model_based', 'model_state', 'right_voltage_error'])
+ .setColor(GREEN)
+ .setDrawLine(false);
+ voltagePlot.addMessageLine(output, ['left_voltage'])
+ .setColor(RED)
+ .setPointSize(0);
+ voltagePlot.addMessageLine(output, ['right_voltage'])
+ .setColor(GREEN)
+ .setPointSize(0);
+
+ // Heading
+ const yawPlot = aosPlotter.addPlot(element);
+ yawPlot.plot.getAxisLabels().setTitle('Robot Yaw');
+ yawPlot.plot.getAxisLabels().setXLabel(TIME);
+ yawPlot.plot.getAxisLabels().setYLabel('Yaw (rad)');
+
+ yawPlot.addMessageLine(status, ['localizer', 'theta']).setColor(GREEN);
+
+ yawPlot.addMessageLine(status, ['down_estimator', 'yaw']).setColor(BLUE);
+
+ yawPlot.addMessageLine(localizer, ['model_based', 'theta']).setColor(RED);
+
+ // Pitch/Roll
+ const orientationPlot = aosPlotter.addPlot(element);
+ orientationPlot.plot.getAxisLabels().setTitle('Orientation');
+ orientationPlot.plot.getAxisLabels().setXLabel(TIME);
+ orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
+
+ orientationPlot.addMessageLine(localizer, ['model_based', 'down_estimator', 'lateral_pitch'])
+ .setColor(RED)
+ .setLabel('roll');
+ orientationPlot
+ .addMessageLine(localizer, ['model_based', 'down_estimator', 'longitudinal_pitch'])
+ .setColor(GREEN)
+ .setLabel('pitch');
+
+ const stillPlot = aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 3]);
+ stillPlot.plot.getAxisLabels().setTitle('Still Plot');
+ stillPlot.plot.getAxisLabels().setXLabel(TIME);
+ stillPlot.plot.getAxisLabels().setYLabel('bool, g\'s');
+ stillPlot.plot.setDefaultYRange([-0.1, 1.1]);
+
+ stillPlot.addMessageLine(localizer, ['model_based', 'down_estimator', 'gravity_magnitude'])
+ .setColor(WHITE)
+ .setDrawLine(false);
+ stillPlot.addMessageLine(localizer, ['model_based', 'using_model'])
+ .setColor(PINK)
+ .setDrawLine(false);
+
+ // Accelerometer/Gravity
+ const accelPlot = aosPlotter.addPlot(element);
+ accelPlot.plot.getAxisLabels().setTitle('Absoluate Velocities')
+ accelPlot.plot.getAxisLabels().setYLabel('Velocity (m/s)');
+ accelPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+
+ accelPlot.addMessageLine(localizer, ['no_wheel_status', 'velocity_x'])
+ .setColor(PINK);
+ accelPlot.addMessageLine(localizer, ['no_wheel_status', 'velocity_y'])
+ .setColor(GREEN);
+ accelPlot.addMessageLine(localizer, ['no_wheel_status', 'velocity_z'])
+ .setColor(BLUE);
+
+ accelPlot.addMessageLine(localizer, ['model_based', 'accel_state', 'velocity_x'])
+ .setColor(RED)
+ .setDrawLine(false);
+ accelPlot.addMessageLine(localizer, ['model_based', 'accel_state', 'velocity_y'])
+ .setColor(GREEN)
+ .setDrawLine(false);
+
+ accelPlot.addMessageLine(localizer, ['model_based', 'oldest_accel_state', 'velocity_x'])
+ .setColor(RED)
+ .setPointSize(0);
+ accelPlot.addMessageLine(localizer, ['model_based', 'oldest_accel_state', 'velocity_y'])
+ .setColor(GREEN)
+ .setPointSize(0);
+
+ // Absolute X Position
+ const xPositionPlot = aosPlotter.addPlot(element);
+ xPositionPlot.plot.getAxisLabels().setTitle('X Position');
+ xPositionPlot.plot.getAxisLabels().setXLabel(TIME);
+ xPositionPlot.plot.getAxisLabels().setYLabel('X Position (m)');
+
+ xPositionPlot.addMessageLine(status, ['x']).setColor(RED);
+ xPositionPlot.addMessageLine(status, ['down_estimator', 'position_x'])
+ .setColor(BLUE);
+ xPositionPlot.addMessageLine(localizer, ['no_wheel_status', 'x']).setColor(GREEN);
+ xPositionPlot.addMessageLine(localizer, ['model_based', 'x']).setColor(CYAN);
+
+ xPositionPlot.plot.setDefaultYRange([0.0, 0.5]);
+
+ // Absolute Y Position
+ const yPositionPlot = aosPlotter.addPlot(element);
+ yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
+ yPositionPlot.plot.getAxisLabels().setXLabel(TIME);
+ yPositionPlot.plot.getAxisLabels().setYLabel('Y Position (m)');
+
+ const localizerY = yPositionPlot.addMessageLine(status, ['y']);
+ localizerY.setColor(RED);
+ yPositionPlot.addMessageLine(status, ['down_estimator', 'position_y'])
+ .setColor(BLUE);
+ yPositionPlot.addMessageLine(localizer, ['no_wheel_status', 'y']).setColor(GREEN);
+ yPositionPlot.addMessageLine(localizer, ['model_based', 'y']).setColor(CYAN);
+
+ // Gyro
+ const gyroPlot = aosPlotter.addPlot(element);
+ gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
+ gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
+ gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const gyroX = gyroPlot.addMessageLine(imu, ['gyro_x']);
+ gyroX.setColor(RED);
+ const gyroY = gyroPlot.addMessageLine(imu, ['gyro_y']);
+ gyroY.setColor(GREEN);
+ const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
+ gyroZ.setColor(BLUE);
+
+ const impliedAccelPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ impliedAccelPlot.plot.getAxisLabels().setTitle('Implied Accelerations');
+ impliedAccelPlot.plot.getAxisLabels().setXLabel(TIME);
+
+ impliedAccelPlot.addMessageLine(localizer, ['model_based', 'implied_accel_z'])
+ .setColor(BLUE);
+ impliedAccelPlot.addMessageLine(localizer, ['model_based', 'implied_accel_y'])
+ .setColor(GREEN);
+ impliedAccelPlot.addMessageLine(localizer, ['model_based', 'implied_accel_x'])
+ .setColor(RED);
+
+ const costPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ costPlot.plot.getAxisLabels().setTitle('Costs');
+ costPlot.plot.getAxisLabels().setXLabel(TIME);
+
+ costPlot.addMessageLine(localizer, ['model_based', 'residual'])
+ .setColor(RED)
+ .setPointSize(0);
+
+ costPlot.addMessageLine(localizer, ['model_based', 'filtered_residual'])
+ .setColor(BLUE)
+ .setPointSize(0);
+
+ costPlot.addMessageLine(localizer, ['model_based', 'velocity_residual'])
+ .setColor(GREEN)
+ .setPointSize(0);
+
+ costPlot.addMessageLine(localizer, ['model_based', 'theta_rate_residual'])
+ .setColor(BROWN)
+ .setPointSize(0);
+
+ costPlot.addMessageLine(localizer, ['model_based', 'accel_residual'])
+ .setColor(CYAN)
+ .setPointSize(0);
+}
diff --git a/y2022/control_loops/localizer/localizer_replay.cc b/y2022/control_loops/localizer/localizer_replay.cc
new file mode 100644
index 0000000..67fb35a
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer_replay.cc
@@ -0,0 +1,119 @@
+
+#include "aos/configuration.h"
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/logging/log_writer.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "aos/json_to_flatbuffer.h"
+#include "aos/network/team_number.h"
+#include "y2022/control_loops/localizer/localizer.h"
+#include "y2022/control_loops/localizer/localizer_schema.h"
+#include "gflags/gflags.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
+
+DEFINE_string(config, "y2020/config.json",
+ "Name of the config file to replay using.");
+DEFINE_int32(team, 7971, "Team number to use for logfile replay.");
+DEFINE_string(output_folder, "/tmp/replayed",
+ "Name of the folder to write replayed logs to.");
+
+class LoggerState {
+ public:
+ LoggerState(aos::logger::LogReader *reader, const aos::Node *node)
+ : event_loop_(
+ reader->event_loop_factory()->MakeEventLoop("logger", node)),
+ namer_(std::make_unique<aos::logger::MultiNodeLogNamer>(
+ absl::StrCat(FLAGS_output_folder, "/", node->name()->string_view(),
+ "/"),
+ event_loop_.get())),
+ logger_(std::make_unique<aos::logger::Logger>(event_loop_.get())) {
+ event_loop_->SkipTimingReport();
+ event_loop_->SkipAosLog();
+ event_loop_->OnRun([this]() { logger_->StartLogging(std::move(namer_)); });
+ }
+
+ private:
+ std::unique_ptr<aos::EventLoop> event_loop_;
+ std::unique_ptr<aos::logger::LogNamer> namer_;
+ std::unique_ptr<aos::logger::Logger> logger_;
+};
+
+// TODO(james): Currently, this replay produces logfiles that can't be read due
+// to time estimation issues. Pending the active refactorings of the
+// timestamp-related code, fix this.
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::network::OverrideTeamNumber(FLAGS_team);
+
+ const aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ // find logfiles
+ std::vector<std::string> unsorted_logfiles =
+ aos::logger::FindLogs(argc, argv);
+
+ // sort logfiles
+ const std::vector<aos::logger::LogFile> logfiles =
+ aos::logger::SortParts(unsorted_logfiles);
+
+ // open logfiles
+ aos::logger::LogReader reader(logfiles);
+ // Patch in any new channels.
+ // TODO(james): With some of the extra changes I've made recently, this is no
+ // longer adequate for replaying old logfiles. Just stop trying to support old
+ // logs.
+ aos::FlatbufferDetachedBuffer<aos::Configuration> updated_config =
+ aos::configuration::MergeWithConfig(
+ reader.configuration(),
+ aos::configuration::AddSchema(
+ R"channel({
+ "channels": [
+ {
+ "name": "/localizer",
+ "type": "frc971.controls.LocalizerStatus",
+ "source_node": "roborio",
+ "frequency": 2000,
+ "max_size": 2000,
+ "num_senders": 2
+ }
+ ]
+})channel",
+ {aos::FlatbufferVector<reflection::Schema>(
+ aos::FlatbufferSpan<reflection::Schema>(
+ frc971::controls::LocalizerStatusSchema()))}));
+
+ auto factory = std::make_unique<aos::SimulatedEventLoopFactory>(
+ &updated_config.message());
+
+ reader.Register(factory.get());
+
+ std::vector<std::unique_ptr<LoggerState>> loggers;
+ // List of nodes to create loggers for (note: currently just roborio; this
+ // code was refactored to allow easily adding new loggers to accommodate
+ // debugging and potential future changes).
+ const std::vector<std::string> nodes_to_log = {"roborio"};
+ for (const std::string &node : nodes_to_log) {
+ loggers.emplace_back(std::make_unique<LoggerState>(
+ &reader, aos::configuration::GetNode(reader.configuration(), node)));
+ }
+
+ const aos::Node *node = nullptr;
+ if (aos::configuration::MultiNode(reader.configuration())) {
+ node = aos::configuration::GetNode(reader.configuration(), "roborio");
+ }
+
+ std::unique_ptr<aos::EventLoop> localizer_event_loop =
+ reader.event_loop_factory()->MakeEventLoop("localizer", node);
+ localizer_event_loop->SkipTimingReport();
+
+ frc971::controls::EventLoopLocalizer localizer(
+ localizer_event_loop.get(),
+ y2020::control_loops::drivetrain::GetDrivetrainConfig());
+
+ reader.event_loop_factory()->Run();
+
+ reader.Deregister();
+
+ return 0;
+}
diff --git a/y2022/control_loops/localizer/localizer_status.fbs b/y2022/control_loops/localizer/localizer_status.fbs
new file mode 100644
index 0000000..6771c5f
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer_status.fbs
@@ -0,0 +1,87 @@
+include "frc971/control_loops/drivetrain/drivetrain_status.fbs";
+
+namespace frc971.controls;
+
+// Stores the state associated with the acceleration-based modelling.
+table AccelBasedState {
+ // x/y position, in meters.
+ x:double (id: 0);
+ y:double (id: 1);
+ // heading, in radians.
+ theta:double (id: 2);
+ // Velocity in X/Y directions, in m/s.
+ velocity_x:double (id: 3);
+ velocity_y:double (id: 4);
+}
+
+// Stores the state associated with the drivetrain model-based state.
+// This model assumes zero lateral motion of the drivetrain.
+table ModelBasedState {
+ // x/y position, in meters.
+ x:double (id: 0);
+ y:double (id: 1);
+ // heading, in radians.
+ theta:double (id: 2);
+ // Expected encoder reading for the left side of the drivetrain, in meters.
+ left_encoder:double (id: 3);
+ // Modelled velocity of the left side of the drivetrain, in meters / second.
+ left_velocity:double (id: 4);
+ // Estimated voltage error, in volts.
+ left_voltage_error:double (id: 5);
+ // Same as the left_* fields, but for the right side of the drivetrain.
+ right_encoder:double (id: 6);
+ right_velocity:double (id: 7);
+ right_voltage_error:double (id: 8);
+}
+
+table ModelBasedStatus {
+ // Current acceleration and model-based states. Depending on using_model,
+ // one of these will be the ground-truth and the other will be calculated
+ // based on it. E.g. if using_model is true, then model_state will be
+ // populated as you'd expect, while accel_state will be populated to be
+ // consistent with model_state (e.g., no lateral motion).
+ accel_state:AccelBasedState (id: 0);
+ model_state:ModelBasedState (id: 1);
+ // using_model indicates whether we are currently in in model-based or
+ // accelerometer-based estimation.
+ using_model:bool (id: 2);
+ // Current residual associated with the amount of inconsistency between
+ // the two models. Will be zero if the drivetrain model is perfectly
+ // consistent with the IMU readings.
+ residual:double (id: 3);
+ // Status from the down estimator.
+ down_estimator:frc971.control_loops.drivetrain.DownEstimatorState (id: 4);
+ // Current ground-truth for x/y/theta. Should match those present in *_state.
+ x:double (id: 5);
+ y:double (id: 6);
+ theta:double (id: 7);
+ // Current accelerations implied by the current accelerometer + down estimator
+ // + yaw readings.
+ implied_accel_x:double (id: 8);
+ implied_accel_y:double (id: 9);
+ implied_accel_z:double (id: 10);
+ // oldest_* are the oldest surviving branches of the model that have just been
+ // running purely on one model.
+ oldest_accel_state:AccelBasedState (id: 11);
+ oldest_model_state:ModelBasedState (id: 12);
+ // Filtered version of the residual field--this is what is actually used by
+ // the code for determining when to swap between modes.
+ filtered_residual:double (id: 13);
+ // Components of the residual. Useful for debugging.
+ velocity_residual:double (id: 14);
+ accel_residual:double (id: 15);
+ theta_rate_residual:double (id: 16);
+ // Number of times we have missed an IMU reading. Should never increase except
+ // *maybe* during startup.
+ clock_resets:int (id: 17);
+}
+
+table LocalizerStatus {
+ model_based:ModelBasedStatus (id: 0);
+ // Whether the IMU is zeroed or not.
+ zeroed:bool (id: 1);
+ // Whether the IMU zeroing is faulted or not.
+ faulted_zero:bool (id: 2);
+}
+
+root_type LocalizerStatus;
diff --git a/y2022/control_loops/localizer/localizer_test.cc b/y2022/control_loops/localizer/localizer_test.cc
new file mode 100644
index 0000000..5857bda
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer_test.cc
@@ -0,0 +1,525 @@
+#include "y2022/control_loops/localizer/localizer.h"
+
+#include "aos/events/simulated_event_loop.h"
+#include "gtest/gtest.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
+
+namespace frc971::controls::testing {
+typedef ModelBasedLocalizer::ModelState ModelState;
+typedef ModelBasedLocalizer::AccelState AccelState;
+typedef ModelBasedLocalizer::ModelInput ModelInput;
+typedef ModelBasedLocalizer::AccelInput AccelInput;
+namespace {
+constexpr size_t kX = ModelBasedLocalizer::kX;
+constexpr size_t kY = ModelBasedLocalizer::kY;
+constexpr size_t kTheta = ModelBasedLocalizer::kTheta;
+constexpr size_t kVelocityX = ModelBasedLocalizer::kVelocityX;
+constexpr size_t kVelocityY = ModelBasedLocalizer::kVelocityY;
+constexpr size_t kAccelX = ModelBasedLocalizer::kAccelX;
+constexpr size_t kAccelY = ModelBasedLocalizer::kAccelY;
+constexpr size_t kThetaRate = ModelBasedLocalizer::kThetaRate;
+constexpr size_t kLeftEncoder = ModelBasedLocalizer::kLeftEncoder;
+constexpr size_t kLeftVelocity = ModelBasedLocalizer::kLeftVelocity;
+constexpr size_t kLeftVoltageError = ModelBasedLocalizer::kLeftVoltageError;
+constexpr size_t kRightEncoder = ModelBasedLocalizer::kRightEncoder;
+constexpr size_t kRightVelocity = ModelBasedLocalizer::kRightVelocity;
+constexpr size_t kRightVoltageError = ModelBasedLocalizer::kRightVoltageError;
+constexpr size_t kLeftVoltage = ModelBasedLocalizer::kLeftVoltage;
+constexpr size_t kRightVoltage = ModelBasedLocalizer::kRightVoltage;
+}
+
+class LocalizerTest : public ::testing::Test {
+ protected:
+ LocalizerTest()
+ : dt_config_(
+ control_loops::drivetrain::testing::GetTestDrivetrainConfig()),
+ localizer_(dt_config_) {}
+ ModelState CallDiffModel(const ModelState &state, const ModelInput &U) {
+ return localizer_.DiffModel(state, U);
+ }
+
+ AccelState CallDiffAccel(const AccelState &state, const AccelInput &U) {
+ return localizer_.DiffAccel(state, U);
+ }
+
+ const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ ModelBasedLocalizer localizer_;
+
+};
+
+TEST_F(LocalizerTest, AccelIntegrationTest) {
+ AccelState state;
+ state.setZero();
+ AccelInput input;
+ input.setZero();
+
+ EXPECT_EQ(0.0, CallDiffAccel(state, input).norm());
+ // Non-zero x/y/theta states should still result in a zero derivative.
+ state(kX) = 1.0;
+ state(kY) = 1.0;
+ state(kTheta) = 1.0;
+ EXPECT_EQ(0.0, CallDiffAccel(state, input).norm());
+
+ state.setZero();
+ state(kVelocityX) = 1.0;
+ state(kVelocityY) = 2.0;
+ EXPECT_EQ((AccelState() << 1.0, 2.0, 0.0, 0.0, 0.0).finished(),
+ CallDiffAccel(state, input));
+ // Derivatives should be independent of theta.
+ state(kTheta) = M_PI / 2.0;
+ EXPECT_EQ((AccelState() << 1.0, 2.0, 0.0, 0.0, 0.0).finished(),
+ CallDiffAccel(state, input));
+
+ state.setZero();
+ input(kAccelX) = 1.0;
+ input(kAccelY) = 2.0;
+ input(kThetaRate) = 3.0;
+ EXPECT_EQ((AccelState() << 0.0, 0.0, 3.0, 1.0, 2.0).finished(),
+ CallDiffAccel(state, input));
+ state(kTheta) = M_PI / 2.0;
+ EXPECT_EQ((AccelState() << 0.0, 0.0, 3.0, 1.0, 2.0).finished(),
+ CallDiffAccel(state, input));
+}
+
+TEST_F(LocalizerTest, ModelIntegrationTest) {
+ ModelState state;
+ state.setZero();
+ ModelInput input;
+ input.setZero();
+ ModelState diff;
+
+ EXPECT_EQ(0.0, CallDiffModel(state, input).norm());
+ // Non-zero x/y/theta/encoder states should still result in a zero derivative.
+ state(kX) = 1.0;
+ state(kY) = 1.0;
+ state(kTheta) = 1.0;
+ state(kLeftEncoder) = 1.0;
+ state(kRightEncoder) = 1.0;
+ EXPECT_EQ(0.0, CallDiffModel(state, input).norm());
+
+ state.setZero();
+ state(kLeftVelocity) = 1.0;
+ state(kRightVelocity) = 1.0;
+ diff = CallDiffModel(state, input);
+ const ModelState mask_velocities =
+ (ModelState() << 1.0, 1.0, 1.0, 1.0, 0.0, 1.0, 1.0, 0.0, 1.0).finished();
+ EXPECT_EQ(
+ (ModelState() << 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0).finished(),
+ diff.cwiseProduct(mask_velocities));
+ EXPECT_EQ(diff(kLeftVelocity), diff(kRightVelocity));
+ EXPECT_GT(0.0, diff(kLeftVelocity));
+ state(kTheta) = M_PI / 2.0;
+ diff = CallDiffModel(state, input);
+ EXPECT_NEAR(0.0,
+ ((ModelState() << 0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0)
+ .finished() -
+ diff.cwiseProduct(mask_velocities))
+ .norm(),
+ 1e-12);
+ EXPECT_EQ(diff(kLeftVelocity), diff(kRightVelocity));
+ EXPECT_GT(0.0, diff(kLeftVelocity));
+
+ state.setZero();
+ state(kLeftVelocity) = -1.0;
+ state(kRightVelocity) = 1.0;
+ diff = CallDiffModel(state, input);
+ EXPECT_EQ((ModelState() << 0.0, 0.0, 1.0 / dt_config_.robot_radius, -1.0, 0.0,
+ 0.0, 1.0, 0.0, 0.0)
+ .finished(),
+ diff.cwiseProduct(mask_velocities));
+ EXPECT_EQ(-diff(kLeftVelocity), diff(kRightVelocity));
+ EXPECT_LT(0.0, diff(kLeftVelocity));
+
+ state.setZero();
+ input(kLeftVoltage) = 5.0;
+ input(kRightVoltage) = 6.0;
+ diff = CallDiffModel(state, input);
+ EXPECT_EQ(0, diff(kX));
+ EXPECT_EQ(0, diff(kY));
+ EXPECT_EQ(0, diff(kTheta));
+ EXPECT_EQ(0, diff(kLeftEncoder));
+ EXPECT_EQ(0, diff(kRightEncoder));
+ EXPECT_EQ(0, diff(kLeftVoltageError));
+ EXPECT_EQ(0, diff(kRightVoltageError));
+ EXPECT_LT(0, diff(kLeftVelocity));
+ EXPECT_LT(0, diff(kRightVelocity));
+ EXPECT_LT(diff(kLeftVelocity), diff(kRightVelocity));
+
+ state.setZero();
+ state(kLeftVoltageError) = -1.0;
+ state(kRightVoltageError) = -2.0;
+ input(kLeftVoltage) = 1.0;
+ input(kRightVoltage) = 2.0;
+ EXPECT_EQ(ModelState::Zero(), CallDiffModel(state, input));
+}
+
+// Test that the HandleReset does indeed reset the state of the localizer.
+TEST_F(LocalizerTest, LocalizerReset) {
+ aos::monotonic_clock::time_point t = aos::monotonic_clock::epoch();
+ localizer_.HandleReset(t, {1.0, 2.0, 3.0});
+ EXPECT_EQ((Eigen::Vector3d{1.0, 2.0, 3.0}), localizer_.xytheta());
+ localizer_.HandleReset(t, {4.0, 5.0, 6.0});
+ EXPECT_EQ((Eigen::Vector3d{4.0, 5.0, 6.0}), localizer_.xytheta());
+}
+
+// Test that if we are moving only by accelerometer readings (and just assuming
+// zero voltage/encoders) that we initially don't believe it but then latch into
+// following the accelerometer.
+// Note: this test is somewhat sensitive to the exact tuning values used for the
+// filter.
+TEST_F(LocalizerTest, AccelOnly) {
+ const aos::monotonic_clock::time_point start = aos::monotonic_clock::epoch();
+ const std::chrono::microseconds kDt{500};
+ aos::monotonic_clock::time_point t = start - std::chrono::milliseconds(1000);
+ Eigen::Vector3d gyro{0.0, 0.0, 0.0};
+ const Eigen::Vector2d encoders{0.0, 0.0};
+ const Eigen::Vector2d voltages{0.0, 0.0};
+ Eigen::Vector3d accel{1.0, 0.2, 9.80665};
+ Eigen::Vector3d accel_gs = accel / 9.80665;
+ while (t < start) {
+ // Spin to fill up the buffer.
+ localizer_.HandleImu(t, gyro, Eigen::Vector3d::UnitZ(), encoders, voltages);
+ t += kDt;
+ }
+ while (t < start + std::chrono::milliseconds(100)) {
+ localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
+ EXPECT_EQ(Eigen::Vector3d::Zero(), localizer_.xytheta());
+ t += kDt;
+ }
+ while (t < start + std::chrono::milliseconds(500)) {
+ // Too lazy to hard-code when the transition happens.
+ localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
+ t += kDt;
+ }
+ while (t < start + std::chrono::milliseconds(1000)) {
+ SCOPED_TRACE(t);
+ localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
+ const Eigen::Vector3d xytheta = localizer_.xytheta();
+ t += kDt;
+ EXPECT_NEAR(
+ 0.5 * accel(0) * std::pow(aos::time::DurationInSeconds(t - start), 2),
+ xytheta(0), 1e-4);
+ EXPECT_NEAR(
+ 0.5 * accel(1) * std::pow(aos::time::DurationInSeconds(t - start), 2),
+ xytheta(1), 1e-4);
+ EXPECT_EQ(0.0, xytheta(2));
+ }
+
+ ASSERT_NEAR(1.0, localizer_.accel_state()(kVelocityX), 1e-10);
+ ASSERT_NEAR(0.2, localizer_.accel_state()(kVelocityY), 1e-10);
+
+ // Start going in a cirlce, and confirm that we
+ // handle things correctly. We rotate the accelerometer readings by 90 degrees
+ // and then leave them constant, which should make it look like we are going
+ // around in a circle.
+ accel = Eigen::Vector3d{-accel(1), accel(0), 9.80665};
+ accel_gs = accel / 9.80665;
+ // v^2 / r = a
+ // w * r = v
+ // v^2 / v * w = a
+ // w = a / v
+ const double omega = accel.topRows<2>().norm() /
+ std::hypot(localizer_.accel_state()(kVelocityX),
+ localizer_.accel_state()(kVelocityY));
+ gyro << 0.0, 0.0, omega;
+ // Due to the magic of math, omega works out to be 1.0 after having run at the
+ // acceleration for one second.
+ ASSERT_NEAR(1.0, omega, 1e-10);
+ // Yes, we could save some operations here, but let's be at least somewhat
+ // clear about what we're doing...
+ const double radius = accel.topRows<2>().norm() / (omega * omega);
+ const Eigen::Vector2d center = localizer_.xytheta().topRows<2>() +
+ accel.topRows<2>().normalized() * radius;
+ const double initial_theta = std::atan2(-accel(1), -accel(0));
+
+ std::chrono::microseconds one_revolution_time(
+ static_cast<int>(2 * M_PI / omega * 1e6));
+
+ aos::monotonic_clock::time_point circle_start = t;
+
+ while (t < circle_start + one_revolution_time) {
+ SCOPED_TRACE(t);
+ localizer_.HandleImu(t, gyro, accel_gs, encoders, voltages);
+ t += kDt;
+ const double t_circle = aos::time::DurationInSeconds(t - circle_start);
+ ASSERT_NEAR(t_circle * omega, localizer_.xytheta()(2), 1e-5);
+ const double theta_circle = t_circle * omega + initial_theta;
+ const Eigen::Vector2d offset =
+ radius *
+ Eigen::Vector2d{std::cos(theta_circle), std::sin(theta_circle)};
+ const Eigen::Vector2d expected = center + offset;
+ const Eigen::Vector2d estimated = localizer_.xytheta().topRows<2>();
+ const Eigen::Vector2d implied_offset = estimated - center;
+ const double implied_theta =
+ std::atan2(implied_offset.y(), implied_offset.x());
+ VLOG(1) << "center: " << center.transpose() << " radius " << radius
+ << "\nlocalizer " << localizer_.xytheta().transpose()
+ << " t_circle " << t_circle << " omega " << omega << " theta "
+ << theta_circle << "\noffset " << offset.transpose()
+ << "\nexpected " << expected.transpose() << "\nimplied offset "
+ << implied_offset << " implied_theta " << implied_theta << "\nvel "
+ << localizer_.accel_state()(kVelocityX) << ", "
+ << localizer_.accel_state()(kVelocityY);
+ ASSERT_NEAR(0.0, (expected - localizer_.xytheta().topRows<2>()).norm(),
+ 1e-2);
+ }
+
+ // Set accelerometer back to zero and confirm that we recover (the
+ // implementation decays the accelerometer speeds to zero when still, so
+ // should recover).
+ while (t <
+ circle_start + one_revolution_time + std::chrono::milliseconds(3000)) {
+ localizer_.HandleImu(t, Eigen::Vector3d::Zero(), Eigen::Vector3d::UnitZ(),
+ encoders, voltages);
+ t += kDt;
+ }
+ const Eigen::Vector3d final_pos = localizer_.xytheta();
+ localizer_.HandleImu(t, Eigen::Vector3d::Zero(), Eigen::Vector3d::UnitZ(),
+ encoders, voltages);
+ ASSERT_NEAR(0.0, (final_pos - localizer_.xytheta()).norm(), 1e-10);
+}
+
+using control_loops::drivetrain::Output;
+
+class EventLoopLocalizerTest : public ::testing::Test {
+ protected:
+ EventLoopLocalizerTest()
+ : configuration_(aos::configuration::ReadConfig("y2022/config.json")),
+ event_loop_factory_(&configuration_.message()),
+ roborio_node_(
+ aos::configuration::GetNode(&configuration_.message(), "roborio")),
+ imu_node_(
+ aos::configuration::GetNode(&configuration_.message(), "imu")),
+ dt_config_(
+ control_loops::drivetrain::testing::GetTestDrivetrainConfig()),
+ localizer_event_loop_(
+ event_loop_factory_.MakeEventLoop("localizer", imu_node_)),
+ localizer_(localizer_event_loop_.get(), dt_config_),
+ drivetrain_plant_event_loop_(event_loop_factory_.MakeEventLoop(
+ "drivetrain_plant", roborio_node_)),
+ drivetrain_plant_imu_event_loop_(
+ event_loop_factory_.MakeEventLoop("drivetrain_plant", imu_node_)),
+ drivetrain_plant_(drivetrain_plant_event_loop_.get(),
+ drivetrain_plant_imu_event_loop_.get(), dt_config_,
+ std::chrono::microseconds(500)),
+ roborio_test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", roborio_node_)),
+ imu_test_event_loop_(
+ event_loop_factory_.MakeEventLoop("test", imu_node_)),
+ logger_test_event_loop_(
+ event_loop_factory_.GetNodeEventLoopFactory("logger")
+ ->MakeEventLoop("test")),
+ output_sender_(
+ roborio_test_event_loop_->MakeSender<Output>("/drivetrain")),
+ output_fetcher_(roborio_test_event_loop_->MakeFetcher<LocalizerOutput>(
+ "/localizer")),
+ status_fetcher_(
+ imu_test_event_loop_->MakeFetcher<LocalizerStatus>("/localizer")) {
+ aos::TimerHandler *timer = roborio_test_event_loop_->AddTimer([this]() {
+ auto builder = output_sender_.MakeBuilder();
+ auto output_builder = builder.MakeBuilder<Output>();
+ output_builder.add_left_voltage(output_voltages_(0));
+ output_builder.add_right_voltage(output_voltages_(1));
+ builder.CheckOk(builder.Send(output_builder.Finish()));
+ });
+ roborio_test_event_loop_->OnRun([timer, this]() {
+ timer->Setup(roborio_test_event_loop_->monotonic_now(),
+ std::chrono::milliseconds(5));
+ });
+ // Get things zeroed.
+ event_loop_factory_.RunFor(std::chrono::seconds(10));
+ CHECK(status_fetcher_.Fetch());
+ CHECK(status_fetcher_->zeroed());
+ }
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> configuration_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+ const aos::Node *const roborio_node_;
+ const aos::Node *const imu_node_;
+ const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
+ std::unique_ptr<aos::EventLoop> localizer_event_loop_;
+ EventLoopLocalizer localizer_;
+
+ std::unique_ptr<aos::EventLoop> drivetrain_plant_event_loop_;
+ std::unique_ptr<aos::EventLoop> drivetrain_plant_imu_event_loop_;
+ control_loops::drivetrain::testing::DrivetrainSimulation drivetrain_plant_;
+
+ std::unique_ptr<aos::EventLoop> roborio_test_event_loop_;
+ std::unique_ptr<aos::EventLoop> imu_test_event_loop_;
+ std::unique_ptr<aos::EventLoop> logger_test_event_loop_;
+
+ aos::Sender<Output> output_sender_;
+ aos::Fetcher<LocalizerOutput> output_fetcher_;
+ aos::Fetcher<LocalizerStatus> status_fetcher_;
+
+ Eigen::Vector2d output_voltages_ = Eigen::Vector2d::Zero();
+};
+
+TEST_F(EventLoopLocalizerTest, Nominal) {
+ output_voltages_ << 1.0, 1.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ drivetrain_plant_.set_accel_sin_magnitude(0.01);
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different times.
+ ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
+ 1e-6);
+ ASSERT_LT(0.1, output_fetcher_->x());
+ ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
+ ASSERT_TRUE(status_fetcher_->has_model_based());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_LT(0.1, status_fetcher_->model_based()->accel_state()->velocity_x());
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(),
+ 1e-10);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
+ 1e-1);
+}
+
+TEST_F(EventLoopLocalizerTest, Reverse) {
+ output_voltages_ << -4.0, -4.0;
+ drivetrain_plant_.set_accel_sin_magnitude(0.01);
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different times.
+ ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
+ 1e-6);
+ ASSERT_GT(-0.1, output_fetcher_->x());
+ ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ ASSERT_NEAR(0.0, output_fetcher_->theta(), 1e-10);
+ ASSERT_TRUE(status_fetcher_->has_model_based());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_GT(-0.1, status_fetcher_->model_based()->accel_state()->velocity_x());
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(),
+ 1e-10);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
+ 1e-1);
+}
+
+TEST_F(EventLoopLocalizerTest, SpinInPlace) {
+ output_voltages_ << 4.0, -4.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different times.
+ ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-6);
+ ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
+ 1e-1);
+ ASSERT_NEAR(0.0, output_fetcher_->x(), 1e-10);
+ ASSERT_NEAR(0.0, output_fetcher_->y(), 1e-10);
+ ASSERT_LT(0.1, std::abs(output_fetcher_->theta()));
+ ASSERT_TRUE(status_fetcher_->has_model_based());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_x(),
+ 1e-10);
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->accel_state()->velocity_y(),
+ 1e-10);
+ ASSERT_NEAR(-status_fetcher_->model_based()->model_state()->left_velocity(),
+ status_fetcher_->model_based()->model_state()->right_velocity(),
+ 1e-3);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->residual(), 1e-3);
+}
+
+TEST_F(EventLoopLocalizerTest, Curve) {
+ output_voltages_ << 2.0, 4.0;
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // The two can be different because they may've been sent at different times.
+ ASSERT_NEAR(output_fetcher_->x(), status_fetcher_->model_based()->x(), 1e-2);
+ ASSERT_NEAR(output_fetcher_->y(), status_fetcher_->model_based()->y(), 1e-2);
+ ASSERT_NEAR(output_fetcher_->theta(), status_fetcher_->model_based()->theta(),
+ 1e-1);
+ ASSERT_LT(0.1, output_fetcher_->x());
+ ASSERT_LT(0.1, output_fetcher_->y());
+ ASSERT_LT(0.1, std::abs(output_fetcher_->theta()));
+ ASSERT_TRUE(status_fetcher_->has_model_based());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_LT(0.0, status_fetcher_->model_based()->accel_state()->velocity_x());
+ ASSERT_LT(0.0, status_fetcher_->model_based()->accel_state()->velocity_y());
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(
+ 0.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
+ 1e-1);
+ ASSERT_NEAR(0.0, status_fetcher_->model_based()->residual(), 1e-1)
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+}
+
+// Tests that small amounts of voltage error are handled by the model-based
+// half of the localizer.
+TEST_F(EventLoopLocalizerTest, VoltageError) {
+ output_voltages_ << 0.0, 0.0;
+ drivetrain_plant_.set_left_voltage_offset(2.0);
+ drivetrain_plant_.set_right_voltage_offset(2.0);
+ drivetrain_plant_.set_accel_sin_magnitude(0.01);
+
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // Should still be using the model, but have a non-trivial residual.
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_LT(0.1, status_fetcher_->model_based()->residual())
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+
+ // Afer running for a while, voltage error terms should converge and result in
+ // low residuals.
+ event_loop_factory_.RunFor(std::chrono::seconds(10));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ ASSERT_TRUE(status_fetcher_->model_based()->using_model());
+ ASSERT_NEAR(
+ 2.0, status_fetcher_->model_based()->model_state()->left_voltage_error(),
+ 0.1)
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+ ASSERT_NEAR(
+ 2.0, status_fetcher_->model_based()->model_state()->right_voltage_error(),
+ 0.1)
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+ ASSERT_GT(0.01, status_fetcher_->model_based()->residual())
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+}
+
+// Tests that large amounts of voltage error force us into the
+// acceleration-based localizer.
+TEST_F(EventLoopLocalizerTest, HighVoltageError) {
+ output_voltages_ << 0.0, 0.0;
+ drivetrain_plant_.set_left_voltage_offset(200.0);
+ drivetrain_plant_.set_right_voltage_offset(200.0);
+ drivetrain_plant_.set_accel_sin_magnitude(0.01);
+
+ event_loop_factory_.RunFor(std::chrono::seconds(2));
+ CHECK(output_fetcher_.Fetch());
+ CHECK(status_fetcher_.Fetch());
+ // Should still be using the model, but have a non-trivial residual.
+ ASSERT_FALSE(status_fetcher_->model_based()->using_model());
+ ASSERT_LT(0.1, status_fetcher_->model_based()->residual())
+ << aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
+ ASSERT_NEAR(drivetrain_plant_.state()(0),
+ status_fetcher_->model_based()->x(), 1.0);
+ ASSERT_NEAR(drivetrain_plant_.state()(1),
+ status_fetcher_->model_based()->y(), 1e-6);
+}
+
+} // namespace frc91::controls::testing
diff --git a/y2022/localizer/imu.cc b/y2022/localizer/imu.cc
index db7ec8f..eb76e00 100644
--- a/y2022/localizer/imu.cc
+++ b/y2022/localizer/imu.cc
@@ -18,7 +18,7 @@
Imu::Imu(aos::ShmEventLoop *event_loop)
: event_loop_(event_loop),
imu_sender_(
- event_loop_->MakeSender<frc971::IMUValuesBatch>("/drivetrain")) {
+ event_loop_->MakeSender<frc971::IMUValuesBatch>("/localizer")) {
event_loop->SetRuntimeRealtimePriority(30);
imu_fd_ = open("/dev/adis16505", O_RDONLY | O_NONBLOCK);
PCHECK(imu_fd_ != -1) << ": Failed to open SPI device for IMU.";
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index c9c2aff..2a6f0a6 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -77,7 +77,7 @@
cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
(void *)image->data()->data());
cv::Mat bgr_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
- cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2RGB_YUYV);
+ cv::cvtColor(image_color_mat, bgr_image, cv::COLOR_YUV2BGR_YUYV);
if (!FLAGS_capture.empty()) {
cv::imwrite(FLAGS_capture, bgr_image);
diff --git a/y2022/y2022.json b/y2022/y2022.json
index 010c675..4fafa8e 100644
--- a/y2022/y2022.json
+++ b/y2022/y2022.json
@@ -18,6 +18,7 @@
"y2022_pi3.json",
"y2022_pi4.json",
"y2022_pi5.json",
+ "y2022_imu.json",
"y2022_logger.json"
]
}
diff --git a/y2022/y2022_imu.json b/y2022/y2022_imu.json
new file mode 100644
index 0000000..9478123
--- /dev/null
+++ b/y2022/y2022_imu.json
@@ -0,0 +1,376 @@
+{
+ "channels": [
+ {
+ "name": "/imu/aos",
+ "type": "aos.timing.Report",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 4096
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "imu",
+ "frequency": 200,
+ "num_senders": 20
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.Status",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 20,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 5,
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 5,
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "imu",
+ "frequency": 15,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio",
+ "logger"
+ ],
+ "max_size": 200,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 1,
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "logger",
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "logger"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/logger/aos/remote_timestamps/imu/logger/aos/aos-starter-StarterRpc",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "logger",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.starter.Status",
+ "source_node": "logger",
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "logger"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/logger/aos/remote_timestamps/imu/logger/aos/aos-starter-Status",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "logger",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "roborio",
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-starter-StarterRpc",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.Status",
+ "source_node": "roborio",
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-starter-Status",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.controls.LocalizerStatus",
+ "source_node": "imu",
+ "frequency": 2200,
+ "max_size": 2000,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio"
+ ],
+ "destination_nodes": [
+ {
+ "name": "logger",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/logger/localizer/frc971-controls-LocalizerStatus",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 2200,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.controls.LocalizerOutput",
+ "source_node": "imu",
+ "frequency": 200,
+ "max_size": 200,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio"
+ ],
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/roborio/localizer/frc971-controls-LocalizerOutput",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/logger/localizer/frc971-controls-LocalizerOutput",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.IMUValuesBatch",
+ "source_node": "imu",
+ "frequency": 2200,
+ "max_size": 1600,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "logger"
+ ],
+ "destination_nodes": [
+ {
+ "name": "logger",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/logger/localizer/frc971-IMUValuesBatch",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 2200,
+ "num_senders": 2,
+ "max_size": 200
+ }
+ ],
+ "applications": [
+ {
+ "name": "message_bridge_client",
+ "executable_name": "message_bridge_client",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "localizer",
+ "executable_name": "localizer",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "localizer_logger",
+ "executable_name": "logger_main",
+ "args": ["--logging_folder", "", "--snappy_compress"],
+ "nodes": [
+ "logger"
+ ]
+ },
+ {
+ "name": "web_proxy",
+ "executable_name": "web_proxy_main",
+ "nodes": [
+ "imu"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/aos"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "imu",
+ "hostname": "imu",
+ "hostnames": [
+ "pi-971-7",
+ "pi-7971-7",
+ "pi-8971-7",
+ "pi-9971-7"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "logger"
+ },
+ {
+ "name": "roborio"
+ }
+ ]
+}
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index 1c71234..592109e 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -20,22 +20,6 @@
},
{
"name": "/drivetrain",
- "type": "frc971.IMUValuesBatch",
- "source_node": "roborio",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 2,
- "time_to_live": 500000000
- }
- ]
- },
- {
- "name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.Position",
"source_node": "roborio",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -268,6 +252,15 @@
]
},
{
+ "name": "imu",
+ "priority": 1,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "logger"
+ ]
+ },
+ {
"name": "roborio",
"priority": 1,
"time_to_live": 5000000,
@@ -288,6 +281,15 @@
"max_size": 200
},
{
+ "name": "/logger/aos/remote_timestamps/imu/logger/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "logger",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
"name": "/logger/aos/remote_timestamps/pi1/logger/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "logger",
@@ -474,6 +476,9 @@
"name": "roborio"
},
{
+ "name": "imu"
+ },
+ {
"name": "pi4"
},
{
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index 24144a8..cb1b2d8 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -138,12 +138,19 @@
"max_size": 208
},
{
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "roborio",
+ "max_size": 208
+ },
+ {
"name": "/roborio/aos",
"type": "aos.message_bridge.Timestamp",
"source_node": "roborio",
"frequency": 15,
"num_senders": 2,
- "max_size": 304,
+ "max_size": 512,
"destination_nodes": [
{
"name": "pi1",
@@ -189,6 +196,15 @@
"roborio"
],
"time_to_live": 5000000
+ },
+ {
+ "name": "imu",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
}
]
},
@@ -204,7 +220,18 @@
"type": "y2022.control_loops.superstructure.Status",
"source_node": "roborio",
"frequency": 200,
- "num_senders": 2
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
},
{
"name": "/superstructure",
@@ -224,14 +251,6 @@
},
{
"name": "/drivetrain",
- "type": "frc971.IMUValuesBatch",
- "source_node": "roborio",
- "frequency": 250,
- "max_size": 2000,
- "num_senders": 2
- },
- {
- "name": "/drivetrain",
"type": "frc971.sensors.GyroReading",
"source_node": "roborio",
"frequency": 200,
@@ -282,7 +301,31 @@
"source_node": "roborio",
"frequency": 200,
"max_size": 80,
- "num_senders": 2
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/drivetrain/frc971-control_loops-drivetrain-Output",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 200
},
{
"name": "/drivetrain",
@@ -297,7 +340,31 @@
"type": "frc971.control_loops.drivetrain.LocalizerControl",
"source_node": "roborio",
"frequency": 200,
- "max_size": 96
+ "max_size": 96,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 0
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/drivetrain/frc971-control_loops-drivetrain-LocalizerControl",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 200
},
{
"name": "/drivetrain",
@@ -435,6 +502,9 @@
"port": 9971
},
{
+ "name": "imu"
+ },
+ {
"name": "logger"
},
{