diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp index 0992476bcf7..1c8074b2735 100644 --- 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 index 3d654e97774..5d0c36515ea 100644 --- a/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp @@ -26,6 +26,7 @@ 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() { @@ -33,7 +34,9 @@ double DutyCycleEncoderSim::GetDistance() { } 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 index 81c27122263..0232faded61 100644 --- 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 index f74333fd13f..be3b90f3645 100644 --- 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 index 683679e33a9..17d8d52722c 100644 --- 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 index f6035dc1705..0c9b885d608 100644 --- 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); + } + } }