commit d5ae85b9c0515c8c74a6b1ae9b1c5ad736bd904e
author Maxwell Henderson <mxwhenderson@gmail.com> Mon Jan 08 16:14:28 2024 -0800
committer Maxwell Henderson <mxwhenderson@gmail.com> Fri Jan 12 16:32:36 2024 -0800
tree 01f5e81e72a60df341bb2638e1546a948909e221
parent 22e466460cc4d28843ec649cc8267d7edcedb2ed

Send drivetrain message in sensor_reader

This moves the logic from sending our drivetrain message in
sensor_reader from wpilib_interface to a helper function in
sensor_reader.h

Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Id5eabc4dceca77038eb656bfe4507fed49031d90

frc971/wpilib/BUILD

diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index ac9446e..aec2a1d 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -401,6 +401,7 @@
         "//aos/time",
         "//aos/util:phased_loop",
         "//frc971/control_loops:control_loops_fbs",
+        "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
         "//third_party:wpilib",
     ],
 )

frc971/wpilib/sensor_reader.cc

diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index bbcc3b5..70d5c35 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -84,6 +84,32 @@
   return monotonic_clock::epoch() + (new_fpga_time + *fpga_offset);
 }
 
+void SensorReader::SendDrivetrainPosition(
+    aos::Sender<control_loops::drivetrain::Position> drivetrain_position_sender,
+    std::function<double(double input)> velocity_translate,
+    std::function<double(double input)> encoder_to_meters, bool left_inverted,
+    bool right_inverted) {
+  auto builder = drivetrain_position_sender.MakeBuilder();
+  frc971::control_loops::drivetrain::Position::Builder drivetrain_builder =
+      builder.MakeBuilder<frc971::control_loops::drivetrain::Position>();
+
+  drivetrain_builder.add_left_encoder(
+      (left_inverted ? -1.0 : 1.0) *
+      encoder_to_meters(drivetrain_left_encoder_->GetRaw()));
+  drivetrain_builder.add_left_speed(
+      (left_inverted ? -1.0 : 1.0) *
+      velocity_translate(drivetrain_left_encoder_->GetPeriod()));
+
+  drivetrain_builder.add_right_encoder(
+      (right_inverted ? -1.0 : 1.0) *
+      encoder_to_meters(drivetrain_right_encoder_->GetRaw()));
+  drivetrain_builder.add_right_speed(
+      (right_inverted ? -1.0 : 1.0) *
+      velocity_translate(drivetrain_right_encoder_->GetPeriod()));
+
+  builder.CheckOk(builder.Send(drivetrain_builder.Finish()));
+}
+
 void SensorReader::DoStart() {
   Start();
   if (dma_synchronizer_) {

frc971/wpilib/sensor_reader.h

diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 59434a8..72a557e 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -9,6 +9,7 @@
 #include "aos/stl_mutex/stl_mutex.h"
 #include "aos/time/time.h"
 #include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
 #include "frc971/input/robot_state_generated.h"
 #include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
 #include "frc971/wpilib/ahal/DigitalInput.h"
@@ -215,6 +216,13 @@
            (2.0 * M_PI);
   }
 
+  void SendDrivetrainPosition(
+      aos::Sender<control_loops::drivetrain::Position>
+          drivetrain_position_sender,
+      std::function<double(double input)> velocity_translate,
+      std::function<double(double input)> encoder_to_meters, bool left_inverted,
+      bool right_inverted);
+
   ::aos::EventLoop *event_loop_;
   ::aos::Sender<::aos::RobotState> robot_state_sender_;