[wpilib] Rewrite DutyCycleEncoder and AnalogEncoder (#6398)

wpilibc/src/main/native/cpp/AnalogEncoder.cpp

@@ -8,130 +8,117 @@
 #include <wpi/sendable/SendableBuilder.h>
 
 #include "frc/AnalogInput.h"
-#include "frc/Counter.h"
 #include "frc/Errors.h"
+#include "frc/MathUtil.h"
 #include "frc/RobotController.h"
 
 using namespace frc;
 
+AnalogEncoder::~AnalogEncoder() {}
+
 AnalogEncoder::AnalogEncoder(int channel)
     : AnalogEncoder(std::make_shared<AnalogInput>(channel)) {}
 
 AnalogEncoder::AnalogEncoder(AnalogInput& analogInput)
-    : m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}},
-      m_analogTrigger{m_analogInput.get()},
-      m_counter{} {
-  Init();
+    : m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}} {
+  Init(1.0, 0.0);
 }
 
 AnalogEncoder::AnalogEncoder(AnalogInput* analogInput)
-    : m_analogInput{analogInput, wpi::NullDeleter<AnalogInput>{}},
-      m_analogTrigger{m_analogInput.get()},
-      m_counter{} {
-  Init();
+    : m_analogInput{analogInput, wpi::NullDeleter<AnalogInput>{}} {
+  Init(1.0, 0.0);
 }
 
 AnalogEncoder::AnalogEncoder(std::shared_ptr<AnalogInput> analogInput)
-    : m_analogInput{std::move(analogInput)},
-      m_analogTrigger{m_analogInput.get()},
-      m_counter{} {
-  Init();
+    : m_analogInput{std::move(analogInput)} {
+  Init(1.0, 0.0);
 }
 
-void AnalogEncoder::Init() {
+AnalogEncoder::AnalogEncoder(int channel, double fullRange, double expectedZero)
+    : AnalogEncoder(std::make_shared<AnalogInput>(channel), fullRange,
+                    expectedZero) {}
+
+AnalogEncoder::AnalogEncoder(AnalogInput& analogInput, double fullRange,
+                             double expectedZero)
+    : m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}} {
+  Init(fullRange, expectedZero);
+}
+
+AnalogEncoder::AnalogEncoder(AnalogInput* analogInput, double fullRange,
+                             double expectedZero)
+    : m_analogInput{analogInput, wpi::NullDeleter<AnalogInput>{}} {
+  Init(fullRange, expectedZero);
+}
+
+AnalogEncoder::AnalogEncoder(std::shared_ptr<AnalogInput> analogInput,
+                             double fullRange, double expectedZero)
+    : m_analogInput{std::move(analogInput)} {
+  Init(fullRange, expectedZero);
+}
+
+void AnalogEncoder::Init(double fullRange, double expectedZero) {
   m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()};
 
   if (m_simDevice) {
     m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
-    m_simAbsolutePosition =
-        m_simDevice.CreateDouble("absPosition", hal::SimDevice::kInput, 0.0);
   }
 
-  m_analogTrigger.SetLimitsVoltage(1.25, 3.75);
-  m_counter.SetUpSource(
-      m_analogTrigger.CreateOutput(AnalogTriggerType::kRisingPulse));
-  m_counter.SetDownSource(
-      m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse));
+  m_fullRange = fullRange;
+  m_expectedZero = expectedZero;
 
-  wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder",
+  wpi::SendableRegistry::AddLW(this, "Analog Encoder",
                                m_analogInput->GetChannel());
 }
 
-static bool DoubleEquals(double a, double b) {
-  constexpr double epsilon = 0.00001;
-  return std::abs(a - b) < epsilon;
-}
-
-units::turn_t AnalogEncoder::Get() const {
+double AnalogEncoder::Get() const {
   if (m_simPosition) {
-    return units::turn_t{m_simPosition.Get()};
+    return m_simPosition.Get();
   }
 
-  // As the values are not atomic, keep trying until we get 2 reads of the same
-  // value If we don't within 10 attempts, error
-  for (int i = 0; i < 10; i++) {
-    auto counter = m_counter.Get();
-    auto pos = m_analogInput->GetVoltage();
-    auto counter2 = m_counter.Get();
-    auto pos2 = m_analogInput->GetVoltage();
-    if (counter == counter2 && DoubleEquals(pos, pos2)) {
-      pos = pos / frc::RobotController::GetVoltage5V();
-      units::turn_t turns{counter + pos - m_positionOffset};
-      m_lastPosition = turns;
-      return turns;
-    }
+  double analog = m_analogInput->GetVoltage();
+  double pos = analog / RobotController::GetVoltage5V();
+
+  // Map sensor range if range isn't full
+  pos = MapSensorRange(pos);
+
+  // Compute full range and offset
+  pos = pos * m_fullRange - m_expectedZero;
+
+  // Map from 0 - Full Range
+  double result = InputModulus(pos, 0.0, m_fullRange);
+  // Invert if necessary
+  if (m_isInverted) {
+    return m_fullRange - result;
   }
-
-  FRC_ReportError(
-      warn::Warning,
-      "Failed to read Analog Encoder. Potential Speed Overrun. Returning last "
-      "value");
-  return m_lastPosition;
+  return result;
 }
 
-double AnalogEncoder::GetAbsolutePosition() const {
-  if (m_simAbsolutePosition) {
-    return m_simAbsolutePosition.Get();
-  }
-
-  return m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V();
+void AnalogEncoder::SetVoltagePercentageRange(double min, double max) {
+  m_sensorMin = std::clamp(min, 0.0, 1.0);
+  m_sensorMax = std::clamp(max, 0.0, 1.0);
 }
 
-double AnalogEncoder::GetPositionOffset() const {
-  return m_positionOffset;
-}
-
-void AnalogEncoder::SetPositionOffset(double offset) {
-  m_positionOffset = std::clamp(offset, 0.0, 1.0);
-}
-
-void AnalogEncoder::SetDistancePerRotation(double distancePerRotation) {
-  m_distancePerRotation = distancePerRotation;
-}
-
-double AnalogEncoder::GetDistancePerRotation() const {
-  return m_distancePerRotation;
-}
-
-double AnalogEncoder::GetDistance() const {
-  return Get().value() * GetDistancePerRotation();
-}
-
-void AnalogEncoder::Reset() {
-  m_counter.Reset();
-  m_positionOffset =
-      m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V();
+void AnalogEncoder::SetInverted(bool inverted) {
+  m_isInverted = inverted;
 }
 
 int AnalogEncoder::GetChannel() const {
   return m_analogInput->GetChannel();
 }
 
+double AnalogEncoder::MapSensorRange(double pos) const {
+  if (pos < m_sensorMin) {
+    pos = m_sensorMin;
+  }
+  if (pos > m_sensorMax) {
+    pos = m_sensorMax;
+  }
+  pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
+  return pos;
+}
+
 void AnalogEncoder::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("AbsoluteEncoder");
   builder.AddDoubleProperty(
-      "Distance", [this] { return this->GetDistance(); }, nullptr);
-  builder.AddDoubleProperty(
-      "Distance Per Rotation",
-      [this] { return this->GetDistancePerRotation(); }, nullptr);
+      "Position", [this] { return this->Get(); }, nullptr);
 }

wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp

@@ -7,108 +7,137 @@
 #include <wpi/NullDeleter.h>
 #include <wpi/sendable/SendableBuilder.h>
 
-#include "frc/Counter.h"
 #include "frc/DigitalInput.h"
 #include "frc/DigitalSource.h"
 #include "frc/DutyCycle.h"
 #include "frc/Errors.h"
+#include "frc/MathUtil.h"
 
 using namespace frc;
 
 DutyCycleEncoder::DutyCycleEncoder(int channel)
     : m_dutyCycle{std::make_shared<DutyCycle>(
           std::make_shared<DigitalInput>(channel))} {
-  Init();
+  Init(1.0, 0.0);
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
     : m_dutyCycle{&dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
-  Init();
+  Init(1.0, 0.0);
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle)
     : m_dutyCycle{dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
-  Init();
+  Init(1.0, 0.0);
 }
 
 DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle)
     : m_dutyCycle{std::move(dutyCycle)} {
-  Init();
+  Init(1.0, 0.0);
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource)
     : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
-  Init();
+  Init(1.0, 0.0);
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource)
     : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
-  Init();
+  Init(1.0, 0.0);
 }
 
 DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource)
     : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
-  Init();
+  Init(1.0, 0.0);
 }
 
-void DutyCycleEncoder::Init() {
+DutyCycleEncoder::DutyCycleEncoder(int channel, double fullRange,
+                                   double expectedZero)
+    : m_dutyCycle{std::make_shared<DutyCycle>(
+          std::make_shared<DigitalInput>(channel))} {
+  Init(fullRange, expectedZero);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle, double fullRange,
+                                   double expectedZero)
+    : m_dutyCycle{&dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
+  Init(fullRange, expectedZero);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle, double fullRange,
+                                   double expectedZero)
+    : m_dutyCycle{dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
+  Init(fullRange, expectedZero);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DutyCycle> dutyCycle,
+                                   double fullRange, double expectedZero)
+    : m_dutyCycle{std::move(dutyCycle)} {
+  Init(fullRange, expectedZero);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DigitalSource& digitalSource,
+                                   double fullRange, double expectedZero)
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
+  Init(fullRange, expectedZero);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(DigitalSource* digitalSource,
+                                   double fullRange, double expectedZero)
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
+  Init(fullRange, expectedZero);
+}
+
+DutyCycleEncoder::DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource,
+                                   double fullRange, double expectedZero)
+    : m_dutyCycle{std::make_shared<DutyCycle>(digitalSource)} {
+  Init(fullRange, expectedZero);
+}
+
+void DutyCycleEncoder::Init(double fullRange, double expectedZero) {
   m_simDevice = hal::SimDevice{"DutyCycle:DutyCycleEncoder",
                                m_dutyCycle->GetSourceChannel()};
 
   if (m_simDevice) {
-    m_simPosition =
-        m_simDevice.CreateDouble("position", hal::SimDevice::kInput, 0.0);
-    m_simDistancePerRotation = m_simDevice.CreateDouble(
-        "distance_per_rot", hal::SimDevice::kOutput, 1.0);
-    m_simAbsolutePosition =
-        m_simDevice.CreateDouble("absPosition", hal::SimDevice::kInput, 0.0);
+    m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
     m_simIsConnected =
-        m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true);
-  } else {
-    m_analogTrigger = std::make_unique<AnalogTrigger>(m_dutyCycle.get());
-    m_analogTrigger->SetLimitsDutyCycle(0.25, 0.75);
-    m_counter = std::make_unique<Counter>();
-    m_counter->SetUpSource(
-        m_analogTrigger->CreateOutput(AnalogTriggerType::kRisingPulse));
-    m_counter->SetDownSource(
-        m_analogTrigger->CreateOutput(AnalogTriggerType::kFallingPulse));
+        m_simDevice.CreateBoolean("Connected", hal::SimDevice::kInput, true);
   }
 
+  m_fullRange = fullRange;
+  m_expectedZero = expectedZero;
+
   wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder",
                                m_dutyCycle->GetSourceChannel());
 }
 
-static bool DoubleEquals(double a, double b) {
-  constexpr double epsilon = 0.00001;
-  return std::abs(a - b) < epsilon;
-}
-
-units::turn_t DutyCycleEncoder::Get() const {
+double DutyCycleEncoder::Get() const {
   if (m_simPosition) {
-    return units::turn_t{m_simPosition.Get()};
+    return m_simPosition.Get();
   }
 
-  // As the values are not atomic, keep trying until we get 2 reads of the same
-  // value If we don't within 10 attempts, error
-  for (int i = 0; i < 10; i++) {
-    auto counter = m_counter->Get();
-    auto pos = m_dutyCycle->GetOutput();
-    auto counter2 = m_counter->Get();
-    auto pos2 = m_dutyCycle->GetOutput();
-    if (counter == counter2 && DoubleEquals(pos, pos2)) {
-      // map sensor range
-      pos = MapSensorRange(pos);
-      units::turn_t turns{counter + pos - m_positionOffset};
-      m_lastPosition = turns;
-      return turns;
-    }
+  double pos;
+  // Compute output percentage (0-1)
+  if (m_period.value() == 0.0) {
+    pos = m_dutyCycle->GetOutput();
+  } else {
+    auto highTime = m_dutyCycle->GetHighTime();
+    pos = highTime / m_period;
   }
 
-  FRC_ReportError(
-      warn::Warning,
-      "Failed to read DutyCycle Encoder. Potential Speed Overrun. Returning "
-      "last value");
-  return m_lastPosition;
+  // Map sensor range if range isn't full
+  pos = MapSensorRange(pos);
+
+  // Compute full range and offset
+  pos = pos * m_fullRange - m_expectedZero;
+
+  // Map from 0 - Full Range
+  double result = InputModulus(pos, 0.0, m_fullRange);
+  // Invert if necessary
+  if (m_isInverted) {
+    return m_fullRange - result;
+  }
+  return result;
 }
 
 double DutyCycleEncoder::MapSensorRange(double pos) const {
@@ -122,54 +151,15 @@ double DutyCycleEncoder::MapSensorRange(double pos) const {
   return pos;
 }
 
-double DutyCycleEncoder::GetAbsolutePosition() const {
-  if (m_simAbsolutePosition) {
-    return m_simAbsolutePosition.Get();
-  }
-
-  return MapSensorRange(m_dutyCycle->GetOutput());
-}
-
-double DutyCycleEncoder::GetPositionOffset() const {
-  return m_positionOffset;
-}
-
-void DutyCycleEncoder::SetPositionOffset(double offset) {
-  m_positionOffset = std::clamp(offset, 0.0, 1.0);
-}
-
 void DutyCycleEncoder::SetDutyCycleRange(double min, double max) {
   m_sensorMin = std::clamp(min, 0.0, 1.0);
   m_sensorMax = std::clamp(max, 0.0, 1.0);
 }
 
-void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) {
-  m_distancePerRotation = distancePerRotation;
-  m_simDistancePerRotation.Set(distancePerRotation);
-}
-
-double DutyCycleEncoder::GetDistancePerRotation() const {
-  return m_distancePerRotation;
-}
-
-double DutyCycleEncoder::GetDistance() const {
-  return Get().value() * GetDistancePerRotation();
-}
-
 int DutyCycleEncoder::GetFrequency() const {
   return m_dutyCycle->GetFrequency();
 }
 
-void DutyCycleEncoder::Reset() {
-  if (m_counter) {
-    m_counter->Reset();
-  }
-  if (m_simPosition) {
-    m_simPosition.Set(0);
-  }
-  m_positionOffset = GetAbsolutePosition();
-}
-
 bool DutyCycleEncoder::IsConnected() const {
   if (m_simIsConnected) {
     return m_simIsConnected.Get();
@@ -184,6 +174,18 @@ void DutyCycleEncoder::SetConnectedFrequencyThreshold(int frequency) {
   m_frequencyThreshold = frequency;
 }
 
+void DutyCycleEncoder::SetInverted(bool inverted) {
+  m_isInverted = inverted;
+}
+
+void DutyCycleEncoder::SetAssumedFrequency(units::hertz_t frequency) {
+  if (frequency.value() == 0) {
+    m_period = 0_s;
+  } else {
+    m_period = 1.0 / frequency;
+  }
+}
+
 int DutyCycleEncoder::GetFPGAIndex() const {
   return m_dutyCycle->GetFPGAIndex();
 }
@@ -195,10 +197,7 @@ int DutyCycleEncoder::GetSourceChannel() const {
 void DutyCycleEncoder::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("AbsoluteEncoder");
   builder.AddDoubleProperty(
-      "Distance", [this] { return this->GetDistance(); }, nullptr);
-  builder.AddDoubleProperty(
-      "Distance Per Rotation",
-      [this] { return this->GetDistancePerRotation(); }, nullptr);
+      "Position", [this] { return this->Get(); }, nullptr);
   builder.AddDoubleProperty(
       "Is Connected", [this] { return this->IsConnected(); }, nullptr);
 }

wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp

@@ -14,18 +14,10 @@ AnalogEncoderSim::AnalogEncoderSim(const frc::AnalogEncoder& encoder) {
   m_positionSim = deviceSim.GetDouble("Position");
 }
 
-void AnalogEncoderSim::SetPosition(frc::Rotation2d angle) {
-  SetTurns(angle.Degrees());
+void AnalogEncoderSim::Set(double value) {
+  m_positionSim.Set(value);
 }
 
-void AnalogEncoderSim::SetTurns(units::turn_t turns) {
-  m_positionSim.Set(turns.value());
-}
-
-units::turn_t AnalogEncoderSim::GetTurns() {
-  return units::turn_t{m_positionSim.Get()};
-}
-
-frc::Rotation2d AnalogEncoderSim::GetPosition() {
-  return units::radian_t{GetTurns()};
+double AnalogEncoderSim::Get() {
+  return m_positionSim.Get();
 }

wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp

@@ -14,38 +14,16 @@ DutyCycleEncoderSim::DutyCycleEncoderSim(const frc::DutyCycleEncoder& encoder)
 
 DutyCycleEncoderSim::DutyCycleEncoderSim(int channel) {
   frc::sim::SimDeviceSim deviceSim{"DutyCycle:DutyCycleEncoder", channel};
-  m_simPosition = deviceSim.GetDouble("position");
-  m_simDistancePerRotation = deviceSim.GetDouble("distance_per_rot");
-  m_simAbsolutePosition = deviceSim.GetDouble("absPosition");
-  m_simIsConnected = deviceSim.GetBoolean("connected");
+  m_simPosition = deviceSim.GetDouble("Position");
+  m_simIsConnected = deviceSim.GetBoolean("Connected");
 }
 
 double DutyCycleEncoderSim::Get() {
   return m_simPosition.Get();
 }
 
-void DutyCycleEncoderSim::Set(units::turn_t turns) {
-  m_simPosition.Set(turns.value());
-}
-
-double DutyCycleEncoderSim::GetDistance() {
-  return m_simPosition.Get() * m_simDistancePerRotation.Get();
-}
-
-void DutyCycleEncoderSim::SetDistance(double distance) {
-  m_simPosition.Set(distance / m_simDistancePerRotation.Get());
-}
-
-double DutyCycleEncoderSim::GetAbsolutePosition() {
-  return m_simAbsolutePosition.Get();
-}
-
-void DutyCycleEncoderSim::SetAbsolutePosition(double position) {
-  m_simAbsolutePosition.Set(position);
-}
-
-double DutyCycleEncoderSim::GetDistancePerRotation() {
-  return m_simDistancePerRotation.Get();
+void DutyCycleEncoderSim::Set(double value) {
+  m_simPosition.Set(value);
 }
 
 bool DutyCycleEncoderSim::IsConnected() {