commit e5f67dd90fb1e57ef9d683560ab16fc960562964
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Feb 12 20:08:29 2022 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Sat Feb 19 14:22:53 2022 -0800
tree 49cb0dd4d555ab11479565988df0d19eee555ffa
parent f7c8a096e178c639cbe5de5980677191cf7ad6b0

Remove unused 2022 channels

IMUValuesBatch on the roborio is unused, I erroneously added a drivetrain
Position message on the imu node earlier, and the drivetrain test
library needed to be updated to support the encoders in the IMUValues
message (as well as the GyroReadings message).

Also, begin forwarding superstructure status message to the IMU pi, and
set the LocalizerControl message to be reliable.

Change-Id: I18f1b4131710e0539018002e91fce99f7f1215b4
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

y2022/control_loops/localizer/localizer.cc

diff --git a/y2022/control_loops/localizer/localizer.cc b/y2022/control_loops/localizer/localizer.cc
index a145ad4..d6afd56 100644
--- a/y2022/control_loops/localizer/localizer.cc
+++ b/y2022/control_loops/localizer/localizer.cc
@@ -507,9 +507,6 @@
       model_based_(dt_config),
       status_sender_(event_loop_->MakeSender<LocalizerStatus>("/localizer")),
       output_sender_(event_loop_->MakeSender<LocalizerOutput>("/localizer")),
-      position_fetcher_(
-          event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Position>(
-              "/localizer")),
       output_fetcher_(
           event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
               "/drivetrain")) {
@@ -526,26 +523,24 @@
   event_loop_->MakeWatcher(
       "/localizer", [this](const frc971::IMUValuesBatch &values) {
         CHECK(values.has_readings());
-        position_fetcher_.Fetch();
         output_fetcher_.Fetch();
         for (const IMUValues *value : *values.readings()) {
           zeroer_.InsertAndProcessMeasurement(*value);
           if (zeroer_.Zeroed()) {
-            CHECK(position_fetcher_.get() != nullptr);
-            CHECK(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(),
-                {position_fetcher_->left_encoder(),
-                 position_fetcher_->right_encoder()},
-                disabled ? Eigen::Vector2d::Zero()
-                         : Eigen::Vector2d{output_fetcher_->left_voltage(),
-                                           output_fetcher_->right_voltage()});
+            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();