Rewrite DutyCycleEncoder and AnalogEncoder

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() {
-  m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()};
+AnalogEncoder::AnalogEncoder(int channel, double fullRange, double expectedZero)
+    : AnalogEncoder(std::make_shared<AnalogInput>(channel), fullRange,
+                    expectedZero) {}
 
-  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));
+AnalogEncoder::AnalogEncoder(AnalogInput& analogInput, double fullRange,
+                             double expectedZero)
+    : m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}} {
+  Init(fullRange, expectedZero);
+}
 
-  wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder",
-                               m_analogInput->GetChannel());
+AnalogEncoder::AnalogEncoder(AnalogInput* analogInput, double fullRange,
+                             double expectedZero)
+    : m_analogInput{analogInput, wpi::NullDeleter<AnalogInput>{}} {
+  Init(fullRange, expectedZero);
 }
 
-static bool DoubleEquals(double a, double b) {
-  constexpr double epsilon = 0.00001;
-  return std::abs(a - b) < epsilon;
+AnalogEncoder::AnalogEncoder(std::shared_ptr<AnalogInput> analogInput,
+                             double fullRange, double expectedZero)
+    : m_analogInput{std::move(analogInput)} {
+  Init(fullRange, expectedZero);
 }
 
-units::turn_t AnalogEncoder::Get() const {
-  if (m_simPosition) {
-    return units::turn_t{m_simPosition.Get()};
-  }
+void AnalogEncoder::Init(double fullRange, double expectedZero) {
+  m_simDevice = hal::SimDevice{"AnalogEncoder", m_analogInput->GetChannel()};
 
-  // 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;
-    }
+  if (m_simDevice) {
+    m_simPosition = m_simDevice.CreateDouble("Position", false, 0.0);
   }
 
-  FRC_ReportError(
-      warn::Warning,
-      "Failed to read Analog Encoder. Potential Speed Overrun. Returning last "
-      "value");
-  return m_lastPosition;
+  m_fullRange = fullRange;
+  m_expectedZero = expectedZero;
+
+  wpi::SendableRegistry::AddLW(this, "Analog Encoder",
+                               m_analogInput->GetChannel());
 }
 
-double AnalogEncoder::GetAbsolutePosition() const {
-  if (m_simAbsolutePosition) {
-    return m_simAbsolutePosition.Get();
+double AnalogEncoder::Get() const {
+  if (m_simPosition) {
+    return m_simPosition.Get();
   }
 
-  return m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V();
-}
+  double analog = m_analogInput->GetVoltage();
+  double pos = analog / RobotController::GetVoltage5V();
 
-double AnalogEncoder::GetPositionOffset() const {
-  return m_positionOffset;
-}
+  // Map sensor range if range isn't full
+  pos = MapSensorRange(pos);
 
-void AnalogEncoder::SetPositionOffset(double offset) {
-  m_positionOffset = std::clamp(offset, 0.0, 1.0);
-}
-
-void AnalogEncoder::SetDistancePerRotation(double distancePerRotation) {
-  m_distancePerRotation = distancePerRotation;
-}
+  // Compute full range and offset
+  pos = pos * m_fullRange - m_expectedZero;
 
-double AnalogEncoder::GetDistancePerRotation() const {
-  return m_distancePerRotation;
+  // 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 AnalogEncoder::GetDistance() const {
-  return Get().value() * GetDistancePerRotation();
+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);
 }
 
-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);
 }