[wpilib] Absolute Encoder API and behavior fixes (#4052)

SetPositionOffset was added. Been requested multiple times, and easy to implement.

The javadocs mentioned GetPositionInRotation. It has tripped up many people how to get the absolute position from the encoder (You currently have to have precreated the DutyCycle object). Add this method (as GetAbsolutePostition) to make this easier to do.

The checks for making sure a matching set of values was read was doing direct double comparisions. This worked ok in the DutyCycle case, but has problems in the analog case. Solve this by using an epsilon comparison.

And finally, scale AnalogEncoders analog input to 0-1 instead of 0-5. This was reported a few years ago, but the issue was missed. This caused the encoder to count from 0-5, then 1-6, then 2-7 etc. This is solved and now works correctly.

Closes #3188
Closes #4046
Closes #4051

And fixes the following issue on CD
https://www.chiefdelphi.com/t/wpilib-analogencoder-java/372649

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

@@ -10,6 +10,7 @@
 #include "frc/AnalogInput.h"
 #include "frc/Counter.h"
 #include "frc/Errors.h"
+#include "frc/RobotController.h"
 
 using namespace frc;
 
@@ -42,6 +43,8 @@ void AnalogEncoder::Init() {
 
   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);
@@ -54,6 +57,11 @@ void AnalogEncoder::Init() {
                                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 {
   if (m_simPosition) {
     return units::turn_t{m_simPosition.Get()};
@@ -66,7 +74,8 @@ units::turn_t AnalogEncoder::Get() const {
     auto pos = m_analogInput->GetVoltage();
     auto counter2 = m_counter.Get();
     auto pos2 = m_analogInput->GetVoltage();
-    if (counter == counter2 && pos == pos2) {
+    if (counter == counter2 && DoubleEquals(pos, pos2)) {
+      pos = pos / frc::RobotController::GetVoltage5V();
       units::turn_t turns{counter + pos - m_positionOffset};
       m_lastPosition = turns;
       return turns;
@@ -80,10 +89,22 @@ units::turn_t AnalogEncoder::Get() const {
   return m_lastPosition;
 }
 
+double AnalogEncoder::GetAbsolutePosition() const {
+  if (m_simAbsolutePosition) {
+    return m_simAbsolutePosition.Get();
+  }
+
+  return m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V();
+}
+
 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;
 }
@@ -98,7 +119,8 @@ double AnalogEncoder::GetDistance() const {
 
 void AnalogEncoder::Reset() {
   m_counter.Reset();
-  m_positionOffset = m_analogInput->GetVoltage();
+  m_positionOffset =
+      m_analogInput->GetVoltage() / frc::RobotController::GetVoltage5V();
 }
 
 int AnalogEncoder::GetChannel() const {

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

@@ -60,6 +60,8 @@ void DutyCycleEncoder::Init() {
         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_simIsConnected =
         m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true);
   } else {
@@ -76,6 +78,11 @@ void DutyCycleEncoder::Init() {
                                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 {
   if (m_simPosition) {
     return units::turn_t{m_simPosition.Get()};
@@ -88,15 +95,9 @@ units::turn_t DutyCycleEncoder::Get() const {
     auto pos = m_dutyCycle->GetOutput();
     auto counter2 = m_counter->Get();
     auto pos2 = m_dutyCycle->GetOutput();
-    if (counter == counter2 && pos == pos2) {
+    if (counter == counter2 && DoubleEquals(pos, pos2)) {
       // map sensor range
-      if (pos < m_sensorMin) {
-        pos = m_sensorMin;
-      }
-      if (pos > m_sensorMax) {
-        pos = m_sensorMax;
-      }
-      pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
+      pos = MapSensorRange(pos);
       units::turn_t turns{counter + pos - m_positionOffset};
       m_lastPosition = turns;
       return turns;
@@ -110,6 +111,33 @@ units::turn_t DutyCycleEncoder::Get() const {
   return m_lastPosition;
 }
 
+double DutyCycleEncoder::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;
+}
+
+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);

wpilibc/src/main/native/include/frc/AnalogEncoder.h

@@ -71,10 +71,24 @@ class AnalogEncoder : public wpi::Sendable,
    */
   units::turn_t Get() const;
 
+  /**
+   * Get the absolute position of the analog encoder.
+   *
+   * <p>GetAbsolutePosition() - GetPositionOffset() will give an encoder
+   * absolute position relative to the last reset. This could potentially be
+   * negative, which needs to be accounted for.
+   *
+   * <p>This will not account for rollovers, and will always be just the raw
+   * absolute position.
+   *
+   * @return the absolute position
+   */
+  double GetAbsolutePosition() const;
+
   /**
    * Get the offset of position relative to the last reset.
    *
-   * GetPositionInRotation() - GetPositionOffset() will give an encoder absolute
+   * GetAbsolutePosition() - GetPositionOffset() will give an encoder absolute
    * position relative to the last reset. This could potentially be negative,
    * which needs to be accounted for.
    *
@@ -82,6 +96,15 @@ class AnalogEncoder : public wpi::Sendable,
    */
   double GetPositionOffset() const;
 
+  /**
+   * Set the position offset.
+   *
+   * <p>This must be in the range of 0-1.
+   *
+   * @param offset the offset
+   */
+  void SetPositionOffset(double offset);
+
   /**
    * Set the distance per rotation of the encoder. This sets the multiplier used
    * to determine the distance driven based on the rotation value from the
@@ -131,5 +154,6 @@ class AnalogEncoder : public wpi::Sendable,
 
   hal::SimDevice m_simDevice;
   hal::SimDouble m_simPosition;
+  hal::SimDouble m_simAbsolutePosition;
 };
 }  // namespace frc

wpilibc/src/main/native/include/frc/DutyCycleEncoder.h

@@ -121,6 +121,40 @@ class DutyCycleEncoder : public wpi::Sendable,
    */
   units::turn_t Get() const;
 
+  /**
+   * Get the absolute position of the duty cycle encoder encoder.
+   *
+   * <p>GetAbsolutePosition() - GetPositionOffset() will give an encoder
+   * absolute position relative to the last reset. This could potentially be
+   * negative, which needs to be accounted for.
+   *
+   * <p>This will not account for rollovers, and will always be just the raw
+   * absolute position.
+   *
+   * @return the absolute position
+   */
+  double GetAbsolutePosition() const;
+
+  /**
+   * Get the offset of position relative to the last reset.
+   *
+   * GetAbsolutePosition() - GetPositionOffset() will give an encoder absolute
+   * position relative to the last reset. This could potentially be negative,
+   * which needs to be accounted for.
+   *
+   * @return the position offset
+   */
+  double GetPositionOffset() const;
+
+  /**
+   * Set the position offset.
+   *
+   * <p>This must be in the range of 0-1.
+   *
+   * @param offset the offset
+   */
+  void SetPositionOffset(double offset);
+
   /**
    * Set the encoder duty cycle range. As the encoder needs to maintain a duty
    * cycle, the duty cycle cannot go all the way to 0% or all the way to 100%.
@@ -182,6 +216,7 @@ class DutyCycleEncoder : public wpi::Sendable,
 
  private:
   void Init();
+  double MapSensorRange(double pos) const;
 
   std::shared_ptr<DutyCycle> m_dutyCycle;
   std::unique_ptr<AnalogTrigger> m_analogTrigger;
@@ -195,6 +230,7 @@ class DutyCycleEncoder : public wpi::Sendable,
 
   hal::SimDevice m_simDevice;
   hal::SimDouble m_simPosition;
+  hal::SimDouble m_simAbsolutePosition;
   hal::SimDouble m_simDistancePerRotation;
   hal::SimBoolean m_simIsConnected;
 };

wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java

@@ -7,6 +7,7 @@ package edu.wpi.first.wpilibj;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDevice.Direction;
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
@@ -23,6 +24,7 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
 
   protected SimDevice m_simDevice;
   protected SimDouble m_simPosition;
+  protected SimDouble m_simAbsolutePosition;
 
   /**
    * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
@@ -51,6 +53,7 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
 
     if (m_simDevice != null) {
       m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0);
+      m_simAbsolutePosition = m_simDevice.createDouble("absPosition", Direction.kInput, 0.0);
     }
 
     // Limits need to be 25% from each end
@@ -61,6 +64,11 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
     SendableRegistry.addLW(this, "Analog Encoder", m_analogInput.getChannel());
   }
 
+  private boolean doubleEquals(double a, double b) {
+    double epsilon = 0.00001d;
+    return Math.abs(a - b) < epsilon;
+  }
+
   /**
    * Get the encoder value since the last reset.
    *
@@ -80,7 +88,8 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
       double pos = m_analogInput.getVoltage();
       double counter2 = m_counter.get();
       double pos2 = m_analogInput.getVoltage();
-      if (counter == counter2 && pos == pos2) {
+      if (counter == counter2 && doubleEquals(pos, pos2)) {
+        pos = pos / RobotController.getVoltage5V();
         double position = counter + pos - m_positionOffset;
         m_lastPosition = position;
         return position;
@@ -92,12 +101,29 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
     return m_lastPosition;
   }
 
+  /**
+   * Get the absolute position of the analog encoder.
+   *
+   * <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
+   * to the last reset. This could potentially be negative, which needs to be accounted for.
+   *
+   * <p>This will not account for rollovers, and will always be just the raw absolute position.
+   *
+   * @return the absolute position
+   */
+  public double getAbsolutePosition() {
+    if (m_simAbsolutePosition != null) {
+      return m_simAbsolutePosition.get();
+    }
+
+    return m_analogInput.getVoltage() / RobotController.getVoltage5V();
+  }
+
   /**
    * Get the offset of position relative to the last reset.
    *
-   * <p>getPositionInRotation() - getPositionOffset() will give an encoder absolute position
-   * relative to the last reset. This could potentially be negative, which needs to be accounted
-   * for.
+   * <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
+   * to the last reset. This could potentially be negative, which needs to be accounted for.
    *
    * @return the position offset
    */
@@ -105,6 +131,17 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
     return m_positionOffset;
   }
 
+  /**
+   * Set the position offset.
+   *
+   * <p>This must be in the range of 0-1.
+   *
+   * @param offset the offset
+   */
+  public void setPositionOffset(double offset) {
+    m_positionOffset = MathUtil.clamp(offset, 0.0, 1.0);
+  }
+
   /**
    * Set the distance per rotation of the encoder. This sets the multiplier used to determine the
    * distance driven based on the rotation value from the encoder. Set this value based on the how
@@ -148,7 +185,7 @@ public class AnalogEncoder implements Sendable, AutoCloseable {
   /** Reset the Encoder distance to zero. */
   public void reset() {
     m_counter.reset();
-    m_positionOffset = m_analogInput.getVoltage();
+    m_positionOffset = m_analogInput.getVoltage() / RobotController.getVoltage5V();
   }
 
   @Override

wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java

@@ -32,6 +32,7 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
 
   protected SimDevice m_simDevice;
   protected SimDouble m_simPosition;
+  protected SimDouble m_simAbsolutePosition;
   protected SimDouble m_simDistancePerRotation;
   protected SimBoolean m_simIsConnected;
 
@@ -75,6 +76,8 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
       m_simPosition = m_simDevice.createDouble("position", SimDevice.Direction.kInput, 0.0);
       m_simDistancePerRotation =
           m_simDevice.createDouble("distance_per_rot", SimDevice.Direction.kOutput, 1.0);
+      m_simAbsolutePosition =
+          m_simDevice.createDouble("absPosition", SimDevice.Direction.kInput, 0.0);
       m_simIsConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true);
     } else {
       m_counter = new Counter();
@@ -87,6 +90,23 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
     SendableRegistry.addLW(this, "DutyCycle Encoder", m_dutyCycle.getSourceChannel());
   }
 
+  private double mapSensorRange(double pos) {
+    // map sensor range
+    if (pos < m_sensorMin) {
+      pos = m_sensorMin;
+    }
+    if (pos > m_sensorMax) {
+      pos = m_sensorMax;
+    }
+    pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
+    return pos;
+  }
+
+  private boolean doubleEquals(double a, double b) {
+    double epsilon = 0.00001d;
+    return Math.abs(a - b) < epsilon;
+  }
+
   /**
    * Get the encoder value since the last reset.
    *
@@ -107,15 +127,9 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
       double pos = m_dutyCycle.getOutput();
       double counter2 = m_counter.get();
       double pos2 = m_dutyCycle.getOutput();
-      if (counter == counter2 && pos == pos2) {
+      if (counter == counter2 && doubleEquals(pos, pos2)) {
         // map sensor range
-        if (pos < m_sensorMin) {
-          pos = m_sensorMin;
-        }
-        if (pos > m_sensorMax) {
-          pos = m_sensorMax;
-        }
-        pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
+        pos = mapSensorRange(pos);
         double position = counter + pos - m_positionOffset;
         m_lastPosition = position;
         return position;
@@ -127,12 +141,29 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
     return m_lastPosition;
   }
 
+  /**
+   * Get the absolute position of the duty cycle encoder.
+   *
+   * <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
+   * to the last reset. This could potentially be negative, which needs to be accounted for.
+   *
+   * <p>This will not account for rollovers, and will always be just the raw absolute position.
+   *
+   * @return the absolute position
+   */
+  public double getAbsolutePosition() {
+    if (m_simAbsolutePosition != null) {
+      return m_simAbsolutePosition.get();
+    }
+
+    return mapSensorRange(m_dutyCycle.getOutput());
+  }
+
   /**
    * Get the offset of position relative to the last reset.
    *
-   * <p>getPositionInRotation() - getPositionOffset() will give an encoder absolute position
-   * relative to the last reset. This could potentially be negative, which needs to be accounted
-   * for.
+   * <p>getAbsolutePosition() - getPositionOffset() will give an encoder absolute position relative
+   * to the last reset. This could potentially be negative, which needs to be accounted for.
    *
    * @return the position offset
    */
@@ -140,6 +171,17 @@ public class DutyCycleEncoder implements Sendable, AutoCloseable {
     return m_positionOffset;
   }
 
+  /**
+   * Set the position offset.
+   *
+   * <p>This must be in the range of 0-1.
+   *
+   * @param offset the offset
+   */
+  public void setPositionOffset(double offset) {
+    m_positionOffset = MathUtil.clamp(offset, 0.0, 1.0);
+  }
+
   /**
    * Set the encoder duty cycle range. As the encoder needs to maintain a duty cycle, the duty cycle
    * cannot go all the way to 0% or all the way to 100%. For example, an encoder with a 4096 us