Add pi localizer binary
Also, run the logger on the imu node.
Change-Id: Iea96906429364777068c95285891ea46644874dc
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/control_loops/localizer/BUILD b/y2022/control_loops/localizer/BUILD
index 3332c1e..034c779 100644
--- a/y2022/control_loops/localizer/BUILD
+++ b/y2022/control_loops/localizer/BUILD
@@ -66,12 +66,23 @@
"//frc971/wpilib:imu_batch_fbs",
"//frc971/wpilib:imu_fbs",
"//frc971/zeroing:imu_zeroer",
- "//y2020/control_loops/drivetrain:drivetrain_base",
"//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"],
@@ -103,5 +114,6 @@
"//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
index dd2e67d..3c68e59 100644
--- a/y2022/control_loops/localizer/localizer.cc
+++ b/y2022/control_loops/localizer/localizer.cc
@@ -23,16 +23,6 @@
}
return vector;
}
-
-#if 0
-Eigen::Matrix<double, 3, 3> AxisToMatrix(const Eigen::Vector3d &vec) {
- const double rotation_norm = vec.norm();
- return rotation_norm < 1e-5
- ? Eigen::Matrix<double, 3, 3>::Identity()
- : Eigen::AngleAxis<double>(rotation_norm, vec / rotation_norm)
- .toRotationMatrix();
-}
-#endif
} // namespace
ModelBasedLocalizer::ModelBasedLocalizer(
@@ -43,6 +33,8 @@
.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();
}
@@ -394,7 +386,11 @@
}
new_branch.accumulated_divergence = 0.0;
- branches_.Push(new_branch);
+ ++branch_counter_;
+ if (branch_counter_ % kBranchPeriod == 0) {
+ branches_.Push(new_branch);
+ branch_counter_ = 0;
+ }
last_residual_ = model_divergence;
diff --git a/y2022/control_loops/localizer/localizer.h b/y2022/control_loops/localizer/localizer.h
index 3a09b28..bb52a40 100644
--- a/y2022/control_loops/localizer/localizer.h
+++ b/y2022/control_loops/localizer/localizer.h
@@ -89,6 +89,9 @@
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;
@@ -176,7 +179,8 @@
// 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> branches_;
+ aos::RingBuffer<CombinedState, 2000 / kBranchPeriod> branches_;
+ int branch_counter_ = 0;
CombinedState current_state_;
aos::monotonic_clock::time_point t_ = aos::monotonic_clock::min_time;
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;
+}