diff --git a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp index c1240da97f..4e70cc21eb 100644 --- a/wpilibc/src/main/native/cpp/AnalogEncoder.cpp +++ b/wpilibc/src/main/native/cpp/AnalogEncoder.cpp @@ -94,6 +94,8 @@ void AnalogEncoder::Reset() { m_positionOffset = m_analogInput->GetVoltage(); } +int AnalogEncoder::GetChannel() const { return m_analogInput->GetChannel(); } + void AnalogEncoder::InitSendable(SendableBuilder& builder) { builder.SetSmartDashboardType("AbsoluteEncoder"); builder.AddDoubleProperty( diff --git a/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp new file mode 100644 index 0000000000..f5c902d557 --- /dev/null +++ b/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/simulation/AnalogEncoderSim.h" + +#include +#include + +#include "frc/AnalogEncoder.h" +#include "frc/simulation/SimDeviceSim.h" + +using namespace frc::sim; + +AnalogEncoderSim::AnalogEncoderSim(const frc::AnalogEncoder& analogEncoder) { + wpi::SmallString<128> fullname; + wpi::raw_svector_ostream os(fullname); + os << "AnalogEncoder" << '[' << analogEncoder.GetChannel() << ']'; + frc::sim::SimDeviceSim deviceSim{fullname.c_str()}; + m_positionSim = deviceSim.GetDouble("Position"); +} + +void AnalogEncoderSim::SetPosition(frc::Rotation2d angle) { + SetTurns(angle.Degrees()); +} + +void AnalogEncoderSim::SetTurns(units::turn_t turns) { + m_positionSim.Set(turns.to()); +} + +units::turn_t AnalogEncoderSim::GetTurns() { + return units::turn_t{m_positionSim.Get()}; +} + +frc::Rotation2d AnalogEncoderSim::GetPosition() { + units::radian_t rads = GetTurns(); + return frc::Rotation2d{rads}; +} diff --git a/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/wpilibc/src/main/native/include/frc/AnalogEncoder.h index efe28c554e..3dc22eaa1d 100644 --- a/wpilibc/src/main/native/include/frc/AnalogEncoder.h +++ b/wpilibc/src/main/native/include/frc/AnalogEncoder.h @@ -108,6 +108,13 @@ class AnalogEncoder : public ErrorBase, */ double GetDistance() const; + /** + * Get the channel number. + * + * @return The channel number. + */ + int GetChannel() const; + void InitSendable(SendableBuilder& builder) override; private: diff --git a/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h new file mode 100644 index 0000000000..debc7c821f --- /dev/null +++ b/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "frc/geometry/Rotation2d.h" + +namespace frc { + +class AnalogEncoder; + +namespace sim { + +/** + * Class to control a simulated analog encoder. + */ +class AnalogEncoderSim { + public: + /** + * Constructs from an AnalogEncoder object. + * + * @param analogInput AnalogEncoder to simulate + */ + explicit AnalogEncoderSim(const AnalogEncoder& analogEncoder); + + /** + * Set the position using an {@link Rotation2d}. + */ + void SetPosition(Rotation2d angle); + + /** + * Set the position of the encoder. + * + * @param turns The position. + */ + void SetTurns(units::turn_t turns); + + /** + * Get the simulated position. + */ + units::turn_t GetTurns(); + + /** + * Get the position as a {@link Rotation2d}. + */ + Rotation2d GetPosition(); + + private: + hal::SimDouble m_positionSim; +}; +} // namespace sim +} // namespace frc diff --git a/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp new file mode 100644 index 0000000000..f32be7efb3 --- /dev/null +++ b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include + +#include "frc/AnalogEncoder.h" +#include "frc/AnalogInput.h" +#include "frc/simulation/AnalogEncoderSim.h" +#include "gtest/gtest.h" + +#define EXPECT_NEAR_UNITS(val1, val2, eps) \ + EXPECT_LE(units::math::abs(val1 - val2), eps) + +TEST(AnalogEncoderSimTest, TestBasic) { + frc::AnalogInput ai(0); + frc::AnalogEncoder encoder{ai}; + frc::sim::AnalogEncoderSim encoderSim{encoder}; + + encoderSim.SetPosition(180_deg); + EXPECT_NEAR(encoder.Get().to(), 0.5, 1E-8); + EXPECT_NEAR(encoderSim.GetTurns().to(), 0.5, 1E-8); + EXPECT_NEAR(encoderSim.GetPosition().Radians().to(), wpi::math::pi, + 1E-8); +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java index 75177e0cd4..bea4e6aab0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -132,6 +132,15 @@ public class AnalogEncoder implements Sendable, AutoCloseable { return get() * getDistancePerRotation(); } + /** + * Get the channel number. + * + * @return The channel number. + */ + public int getChannel() { + return m_analogInput.getChannel(); + } + /** * Reset the Encoder distance to zero. */ diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java new file mode 100644 index 0000000000..a6fb6a9c85 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.wpilibj.AnalogEncoder; +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +/** + * Class to control a simulated analog encoder. + */ +public class AnalogEncoderSim { + private final SimDouble m_simPosition; + + /** + * Constructs from an AnalogEncoder object. + * + * @param encoder AnalogEncoder to simulate + */ + public AnalogEncoderSim(AnalogEncoder encoder) { + SimDeviceSim wrappedSimDevice = new SimDeviceSim("AnalogEncoder" + "[" + encoder.getChannel() + "]"); + m_simPosition = wrappedSimDevice.getDouble("Position"); + } + + /** + * Set the position using an {@link Rotation2d}. + */ + public void setPosition(Rotation2d angle) { + setTurns(angle.getDegrees() / 360.0); + } + + /** + * Set the position of the encoder. + * + * @param turns The position. + */ + public void setTurns(double turns) { + m_simPosition.set(turns); + } + + /** + * Get the simulated position. + */ + public double getTurns() { + return m_simPosition.get(); + } + + /** + * Get the position as a {@link Rotation2d}. + */ + public Rotation2d getPosition() { + return Rotation2d.fromDegrees(getTurns() * 360.0); + } +} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java new file mode 100644 index 0000000000..677c5eb089 --- /dev/null +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.simulation; + +import edu.wpi.first.wpilibj.AnalogEncoder; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import org.junit.jupiter.api.Test; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class AnalogEncoderSimTest { + @Test + public void testBasic() { + var analogInput = new AnalogInput(0); + var analogEncoder = new AnalogEncoder(analogInput); + var encoderSim = new AnalogEncoderSim(analogEncoder); + + encoderSim.setPosition(Rotation2d.fromDegrees(180)); + assertEquals(analogEncoder.get(), 0.5, 1E-8); + assertEquals(encoderSim.getTurns(), 0.5, 1E-8); + assertEquals(encoderSim.getPosition().getRadians(), Math.PI, 1E-8); + } +}