commit 53507e1ce629ff792ff2b079ad6a2fef30e1f3b1
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Feb 12 18:36:40 2022 -0800
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Feb 12 19:39:44 2022 -0800
tree c43e784b00fa66eb80632f3b853b806ae83c4bf8
parent e83c14d9dbaffb8a315d0a095e930cea501df2ed

Send 2022 encoder values in meters

Be more consistent with how the drivetrain position message manages
things and just convert to meters.

This doesn't break logs, because no logfiles with the int-based encoder
counts exist.

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

y2022/constants.h

diff --git a/y2022/constants.h b/y2022/constants.h
index 050c5c7..92f3a48 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -30,6 +30,15 @@
            constants::Values::kDrivetrainEncoderRatio() *
            kDrivetrainEncoderCountsPerRevolution();
   }
+
+  static double DrivetrainEncoderToMeters(int32_t in) {
+    return ((static_cast<double>(in) /
+             kDrivetrainEncoderCountsPerRevolution()) *
+            (2.0 * M_PI)) *
+           kDrivetrainEncoderRatio() *
+           control_loops::drivetrain::kWheelRadius;
+  }
+
   static constexpr double kRollerSupplyCurrentLimit() { return 30.0; }
   static constexpr double kRollerStatorCurrentLimit() { return 40.0; }
 

y2022/localizer/imu.cc

diff --git a/y2022/localizer/imu.cc b/y2022/localizer/imu.cc
index dbdc1a1..db7ec8f 100644
--- a/y2022/localizer/imu.cc
+++ b/y2022/localizer/imu.cc
@@ -2,6 +2,7 @@
 
 #include "aos/util/crc32.h"
 #include "glog/logging.h"
+#include "y2022/constants.h"
 
 namespace y2022::localizer {
 
@@ -120,8 +121,10 @@
 
     // extra data from the pico
     imu_builder.add_pico_timestamp_us(pico_timestamp);
-    imu_builder.add_left_encoder(encoder1_count);
-    imu_builder.add_right_encoder(encoder2_count);
+    imu_builder.add_left_encoder(
+        constants::Values::DrivetrainEncoderToMeters(encoder1_count));
+    imu_builder.add_right_encoder(
+        constants::Values::DrivetrainEncoderToMeters(encoder2_count));
     imu_builder.add_previous_reading_diag_stat(diag_stat_offset);
   }
 

y2022/wpilib_interface.cc

diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 0ee1e92..673fa80 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -68,14 +68,6 @@
 // DMA stuff and then removing the * 2.0 in *_translate.
 // The low bit is direction.
 
-double drivetrain_translate(int32_t in) {
-  return ((static_cast<double>(in) /
-           Values::kDrivetrainEncoderCountsPerRevolution()) *
-          (2.0 * M_PI)) *
-         Values::kDrivetrainEncoderRatio() *
-         control_loops::drivetrain::kWheelRadius;
-}
-
 double drivetrain_velocity_translate(double in) {
   return (((1.0 / in) / Values::kDrivetrainCyclesPerRevolution()) *
           (2.0 * M_PI)) *
@@ -127,12 +119,14 @@
       frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
           builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
       drivetrain_builder.add_left_encoder(
-          drivetrain_translate(drivetrain_left_encoder_->GetRaw()));
+          constants::Values::DrivetrainEncoderToMeters(
+              drivetrain_left_encoder_->GetRaw()));
       drivetrain_builder.add_left_speed(
           drivetrain_velocity_translate(drivetrain_left_encoder_->GetPeriod()));
 
       drivetrain_builder.add_right_encoder(
-          -drivetrain_translate(drivetrain_right_encoder_->GetRaw()));
+          -constants::Values::DrivetrainEncoderToMeters(
+              drivetrain_right_encoder_->GetRaw()));
       drivetrain_builder.add_right_speed(-drivetrain_velocity_translate(
           drivetrain_right_encoder_->GetPeriod()));