Sync absolute position with position

Add reset test
wpilibsuite · Jul 13, 2023 · 65aeb41 · 65aeb41
1 parent 1ea194c
commit 65aeb41
Show file tree

Hide file tree

Showing 6 changed files with 47 additions and 2 deletions.
diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -167,6 +167,10 @@ void DutyCycleEncoder::Reset() {
   if (m_counter) {
     m_counter->Reset();
   }
+  if (m_simPosition) {
+    m_simPosition.Set(0);
+    m_simAbsolutePosition.Set(0);
+  }
   m_positionOffset = GetAbsolutePosition();
 }
 

diff --git a/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp
@@ -26,14 +26,17 @@ double DutyCycleEncoderSim::Get() {
 
 void DutyCycleEncoderSim::Set(units::turn_t turns) {
   m_simPosition.Set(turns.value());
+  m_simAbsolutePosition.Set(std::fmod(turns.value(), 1));
 }
 
 double DutyCycleEncoderSim::GetDistance() {
   return m_simPosition.Get() * m_simDistancePerRotation.Get();
 }
 
 void DutyCycleEncoderSim::SetDistance(double distance) {
-  m_simPosition.Set(distance / m_simDistancePerRotation.Get());
+  double pos = distance / m_simDistancePerRotation.Get();
+  m_simPosition.Set(pos);
+  m_simAbsolutePosition.Set(std::fmod(pos, 1));
 }
 
 double DutyCycleEncoderSim::GetAbsolutePosition() {

diff --git a/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp
@@ -21,6 +21,7 @@ TEST(DutyCycleEncoderSimTest, Set) {
   constexpr units::turn_t kTestValue{5.67};
   sim.Set(kTestValue);
   EXPECT_EQ(kTestValue, enc.Get());
+  EXPECT_NEAR(0.67, enc.GetAbsolutePosition(), 1e-3);
 }
 
 TEST(DutyCycleEncoderSimTest, SetDistance) {
@@ -30,6 +31,7 @@ TEST(DutyCycleEncoderSimTest, SetDistance) {
   DutyCycleEncoderSim sim(enc);
   sim.SetDistance(19.1);
   EXPECT_EQ(19.1, enc.GetDistance());
+  EXPECT_NEAR(0.1, enc.GetAbsolutePosition(), 1e-3);
 }
 
 TEST(DutyCycleEncoderSimTest, SetDistancePerRotation) {
@@ -62,4 +64,16 @@ TEST(DutyCycleEncoderSimTest, SetIsConnected) {
   EXPECT_FALSE(enc.IsConnected());
 }
 
+TEST(DutyCycleEncoderSimTest, Reset) {
+  HAL_Initialize(500, 0);
+
+  DutyCycleEncoder enc{0};
+  DutyCycleEncoderSim sim(enc);
+  sim.SetDistance(2.5);
+  EXPECT_EQ(2.5, enc.GetDistance());
+  enc.Reset();
+  EXPECT_EQ(0, enc.GetDistance());
+  EXPECT_EQ(0, enc.GetAbsolutePosition());
+}
+
 }  // namespace frc::sim
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
@@ -249,6 +249,10 @@ public void reset() {
     if (m_counter != null) {
       m_counter.reset();
     }
+    if (m_simPosition != null) {
+      m_simPosition.set(0);
+      m_simAbsolutePosition.set(0);
+    }
     m_positionOffset = getAbsolutePosition();
   }
 

diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java
@@ -53,6 +53,7 @@ public double get() {
    */
   public void set(double turns) {
     m_simPosition.set(turns);
+    m_simAbsolutePosition.set(turns % 1);
   }
 
   /**
@@ -70,7 +71,9 @@ public double getDistance() {
    * @param distance The distance.
    */
   public void setDistance(double distance) {
-    m_simPosition.set(distance / m_simDistancePerRotation.get());
+    double pos = distance / m_simDistancePerRotation.get();
+    m_simPosition.set(pos);
+    m_simAbsolutePosition.set(pos % 1);
   }
 
   /**

diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java
@@ -20,6 +20,7 @@ void setTest() {
 
       sim.set(5.67);
       assertEquals(5.67, encoder.get());
+      assertEquals(0.67, encoder.getAbsolutePosition(), 1e-3);
     }
   }
 
@@ -32,6 +33,7 @@ void setDistanceTest() {
 
       sim.setDistance(19.1);
       assertEquals(19.1, encoder.getDistance());
+      assertEquals(0.1, encoder.getAbsolutePosition(), 1e-10);
     }
   }
 
@@ -72,4 +74,19 @@ void setIsConnectedTest() {
       assertFalse(encoder.isConnected());
     }
   }
+
+  @Test
+  void resetTest() {
+    HAL.initialize(500, 0);
+
+    try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) {
+      DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder);
+
+      sim.setDistance(2.5);
+      assertEquals(2.5, encoder.getDistance());
+      encoder.reset();
+      assertEquals(0.0, encoder.getDistance());
+      assertEquals(0.0, encoder.getAbsolutePosition(), 1e-3);
+    }
+  }
 }