commit 5e7b6e3e0b3e9f635c250bf9c5075de323632318
author Maxwell Henderson <mxwhenderson@gmail.com> Tue Feb 20 15:30:33 2024 -0800
committer Maxwell Henderson <mxwhenderson@gmail.com> Tue Feb 20 15:31:52 2024 -0800
tree c2cd47a0559944fd6d34483c2b266d156b96c87d
parent f7e0a3002f18cc81bc7d2f308644095872f255a4

Add a DMAAbsoluteEncoderAndPotentiometer subsystem

This lets us use AbsoluteEncoderAndPotentiometer with a DMA absolute_pwm

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

frc971/wpilib/encoder_and_potentiometer.h

diff --git a/frc971/wpilib/encoder_and_potentiometer.h b/frc971/wpilib/encoder_and_potentiometer.h
index a5eac24..84bfb64 100644
--- a/frc971/wpilib/encoder_and_potentiometer.h
+++ b/frc971/wpilib/encoder_and_potentiometer.h
@@ -9,6 +9,7 @@
 #include "aos/time/time.h"
 #include "frc971/wpilib/ahal/AnalogInput.h"
 #include "frc971/wpilib/ahal/Counter.h"
+#include "frc971/wpilib/ahal/DigitalInput.h"
 #include "frc971/wpilib/ahal/DigitalSource.h"
 #include "frc971/wpilib/ahal/Encoder.h"
 #include "frc971/wpilib/dma.h"
@@ -168,6 +169,38 @@
   ::std::unique_ptr<::frc::DigitalInput> input_;
 };
 
+class DMAAbsoluteEncoderAndPotentiometer {
+ public:
+  void set_absolute_pwm(::std::unique_ptr<frc::DigitalInput> input) {
+    duty_cycle_input_ = ::std::move(input);
+    duty_cycle_reader_.set_input(duty_cycle_input_.get());
+  }
+
+  void set_encoder(::std::unique_ptr<frc::Encoder> encoder) {
+    encoder_ = ::std::move(encoder);
+  }
+
+  void set_potentiometer(::std::unique_ptr<frc::AnalogInput> potentiometer) {
+    potentiometer_ = ::std::move(potentiometer);
+  }
+
+  double ReadAbsoluteEncoder() const {
+    return duty_cycle_reader_.last_width() / duty_cycle_reader_.last_period();
+  }
+  int32_t ReadRelativeEncoder() const { return encoder_->GetRaw(); }
+  double ReadPotentiometerVoltage() const {
+    return potentiometer_->GetVoltage();
+  }
+
+  DMAPulseWidthReader &reader() { return duty_cycle_reader_; }
+
+ private:
+  DMAPulseWidthReader duty_cycle_reader_;
+  ::std::unique_ptr<::frc::DigitalInput> duty_cycle_input_;
+  ::std::unique_ptr<frc::Encoder> encoder_;
+  ::std::unique_ptr<frc::AnalogInput> potentiometer_;
+};
+
 // Class to hold a CTRE encoder with absolute angle pwm and potentiometer pair.
 class AbsoluteEncoderAndPotentiometer {
  public:

frc971/wpilib/sensor_reader.h

diff --git a/frc971/wpilib/sensor_reader.h b/frc971/wpilib/sensor_reader.h
index 455ed0e..da650a5 100644
--- a/frc971/wpilib/sensor_reader.h
+++ b/frc971/wpilib/sensor_reader.h
@@ -102,6 +102,27 @@
                                    encoder_ratio * (2.0 * M_PI));
   }
 
+  void CopyPosition(
+      const ::frc971::wpilib::DMAAbsoluteEncoderAndPotentiometer &encoder,
+      ::frc971::PotAndAbsolutePositionStatic *position,
+      double encoder_counts_per_revolution, double encoder_ratio,
+      ::std::function<double(double)> potentiometer_translate, bool reverse,
+      double pot_offset) {
+    const double multiplier = reverse ? -1.0 : 1.0;
+    position->set_pot(multiplier * potentiometer_translate(
+                                       encoder.ReadPotentiometerVoltage()) +
+                      pot_offset);
+    position->set_encoder(multiplier *
+                          encoder_translate(encoder.ReadRelativeEncoder(),
+                                            encoder_counts_per_revolution,
+                                            encoder_ratio));
+
+    position->set_absolute_encoder((reverse
+                                        ? (1.0 - encoder.ReadAbsoluteEncoder())
+                                        : encoder.ReadAbsoluteEncoder()) *
+                                   encoder_ratio * (2.0 * M_PI));
+  }
+
   // Copies an AbsoluteEncoderAndPotentiometer to an AbsoluteAndAbsolutePosition
   // with the correct unit and direction changes.
   void CopyPosition(const ::frc971::wpilib::AbsoluteAndAbsoluteEncoder &encoder,