diff --git a/wpilibc/src/main/native/include/frc/interfaces/Gyro.h b/wpilibc/src/main/native/include/frc/interfaces/Gyro.h index b74a3cf5d6..7e322c4313 100644 --- a/wpilibc/src/main/native/include/frc/interfaces/Gyro.h +++ b/wpilibc/src/main/native/include/frc/interfaces/Gyro.h @@ -66,10 +66,6 @@ class Gyro { /** * Return the heading of the robot as a Rotation2d. * - * The angle is continuous, that is it will continue from 360 to 361 degrees. - * This allows algorithms that wouldn't want to see a discontinuity in the - * gyro output as it sweeps past from 360 to 0 on the second time around. - * * The angle is expected to increase as the gyro turns counterclockwise when * looked at from the top. It needs to follow the NWU axis convention. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java index 27377be03e..1d1c919d57 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java @@ -52,10 +52,6 @@ public interface Gyro extends AutoCloseable { /** * Return the heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}. * - *

The angle is continuous, that is it will continue from 360 to 361 degrees. This allows - * algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from - * 360 to 0 on the second time around. - * *

The angle is expected to increase as the gyro turns counterclockwise when looked at from the * top. It needs to follow the NWU axis convention. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index b4bbe0042c..8845ea5f9b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -19,13 +19,11 @@ import java.util.Objects; @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation2d implements Interpolatable { - private final double m_value; private final double m_cos; private final double m_sin; /** Constructs a Rotation2d with a default angle of 0 degrees. */ public Rotation2d() { - m_value = 0.0; m_cos = 1.0; m_sin = 0.0; } @@ -37,9 +35,7 @@ public class Rotation2d implements Interpolatable { */ @JsonCreator public Rotation2d(@JsonProperty(required = true, value = "radians") double value) { - m_value = value; - m_cos = Math.cos(value); - m_sin = Math.sin(value); + this(Math.cos(value), Math.sin(value)); } /** @@ -57,7 +53,6 @@ public class Rotation2d implements Interpolatable { m_sin = 0.0; m_cos = 1.0; } - m_value = Math.atan2(m_sin, m_cos); } /** @@ -123,7 +118,7 @@ public class Rotation2d implements Interpolatable { * @return The inverse of the current rotation. */ public Rotation2d unaryMinus() { - return new Rotation2d(-m_value); + return new Rotation2d(m_cos, -m_sin); } /** @@ -133,7 +128,7 @@ public class Rotation2d implements Interpolatable { * @return The new scaled Rotation2d. */ public Rotation2d times(double scalar) { - return new Rotation2d(m_value * scalar); + return new Rotation2d(getRadians() * scalar); } /** @@ -166,22 +161,22 @@ public class Rotation2d implements Interpolatable { } /** - * Returns the radian value of the Rotation2d. + * Returns the radian value of the Rotation2d within (-pi, pi]. * * @return The radian value of the Rotation2d. */ @JsonProperty public double getRadians() { - return m_value; + return Math.atan2(m_sin, m_cos); } /** - * Returns the degree value of the Rotation2d. + * Returns the degree value of the Rotation2d within (-180, 180]. * * @return The degree value of the Rotation2d. */ public double getDegrees() { - return Math.toDegrees(m_value); + return Math.toDegrees(getRadians()); } /** @@ -190,7 +185,7 @@ public class Rotation2d implements Interpolatable { * @return The number of rotations of the Rotation2d. */ public double getRotations() { - return Units.radiansToRotations(m_value); + return Units.radiansToRotations(getRadians()); } /** @@ -222,7 +217,7 @@ public class Rotation2d implements Interpolatable { @Override public String toString() { - return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value)); + return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", getRadians(), getDegrees()); } /** @@ -242,7 +237,7 @@ public class Rotation2d implements Interpolatable { @Override public int hashCode() { - return Objects.hash(m_value); + return Objects.hash(getRadians()); } @Override diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index 3a4ea4d70e..11074af9a8 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -132,18 +132,18 @@ class WPILIB_DLLEXPORT Rotation2d { constexpr Rotation2d RotateBy(const Rotation2d& other) const; /** - * Returns the radian value of the rotation. + * Returns the radian value of the rotation within (-pi, pi]. * * @return The radian value of the rotation. */ - constexpr units::radian_t Radians() const { return m_value; } + constexpr units::radian_t Radians() const; /** - * Returns the degree value of the rotation. + * Returns the degree value of the rotation within (-180, 180]. * * @return The degree value of the rotation. */ - constexpr units::degree_t Degrees() const { return m_value; } + constexpr units::degree_t Degrees() const; /** * Returns the cosine of the rotation. @@ -167,7 +167,6 @@ class WPILIB_DLLEXPORT Rotation2d { constexpr double Tan() const { return Sin() / Cos(); } private: - units::radian_t m_value = 0_rad; double m_cos = 1; double m_sin = 0; }; diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc index ac3f1f5e59..5399c89f90 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc @@ -13,11 +13,10 @@ namespace frc { constexpr Rotation2d::Rotation2d(units::radian_t value) - : m_value(value), - m_cos(std::is_constant_evaluated() ? gcem::cos(value.to()) - : std::cos(value.to())), - m_sin(std::is_constant_evaluated() ? gcem::sin(value.to()) - : std::sin(value.to())) {} + : Rotation2d(std::is_constant_evaluated() ? gcem::cos(value.to()) + : std::cos(value.to()), + std::is_constant_evaluated() ? gcem::sin(value.to()) + : std::sin(value.to())) {} constexpr Rotation2d::Rotation2d(units::degree_t value) : Rotation2d(units::radian_t{value}) {} @@ -31,17 +30,14 @@ constexpr Rotation2d::Rotation2d(double x, double y) { m_sin = 0.0; m_cos = 1.0; } - m_value = - units::radian_t{std::is_constant_evaluated() ? gcem::atan2(m_sin, m_cos) - : std::atan2(m_sin, m_cos)}; } constexpr Rotation2d Rotation2d::operator-() const { - return Rotation2d{-m_value}; + return Rotation2d{m_cos, -m_sin}; } constexpr Rotation2d Rotation2d::operator*(double scalar) const { - return Rotation2d(m_value * scalar); + return Rotation2d(Radians() * scalar); } constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const { @@ -71,4 +67,14 @@ constexpr Rotation2d Rotation2d::RotateBy(const Rotation2d& other) const { Cos() * other.Sin() + Sin() * other.Cos()}; } +constexpr units::radian_t Rotation2d::Radians() const { + return units::radian_t{std::is_constant_evaluated() + ? gcem::atan2(m_sin, m_cos) + : std::atan2(m_sin, m_cos)}; +} + +constexpr units::degree_t Rotation2d::Degrees() const { + return Radians(); +} + } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java index f960e493cb..c6e75d4741 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java @@ -13,6 +13,15 @@ import org.junit.jupiter.api.Test; class Rotation2dTest { private static final double kEpsilon = 1E-9; + @Test + void testInScope() { + var rot1 = Rotation2d.fromRadians(Math.PI * 5 / 2); + var rot2 = Rotation2d.fromRadians(Math.PI * 7 / 2); + + assertEquals(Math.PI / 2, rot1.getRadians(), kEpsilon); + assertEquals(-Math.PI / 2, rot2.getRadians(), kEpsilon); + } + @Test void testRadiansToDegrees() { var rot1 = Rotation2d.fromRadians(Math.PI / 3); @@ -59,6 +68,21 @@ class Rotation2dTest { assertEquals(40.0, rot1.minus(rot2).getDegrees(), kEpsilon); } + @Test + void testUnaryMinus() { + var rot = Rotation2d.fromDegrees(20.0); + + assertEquals(-20.0, rot.unaryMinus().getDegrees(), kEpsilon); + } + + @Test + void testMultiply() { + var rot = Rotation2d.fromDegrees(10.0); + + assertEquals(30.0, rot.times(3.0).getDegrees(), kEpsilon); + assertEquals(50.0, rot.times(41.0).getDegrees(), kEpsilon); + } + @Test void testEquality() { var rot1 = Rotation2d.fromDegrees(43.0); diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 3fdd37a3ff..25cbecf9b6 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -10,6 +10,16 @@ using namespace frc; +TEST(Rotation2dTest, InScope) { + const auto rot1 = Rotation2d{units::radian_t{std::numbers::pi * 5 / 2}}; + const auto rot2 = Rotation2d{units::radian_t{std::numbers::pi * 7 / 2}}; + const auto rot3 = Rotation2d{270_deg}; + + EXPECT_DOUBLE_EQ(std::numbers::pi / 2, rot1.Radians().value()); + EXPECT_DOUBLE_EQ(-std::numbers::pi / 2, rot2.Radians().value()); + EXPECT_DOUBLE_EQ(-90, rot3.Degrees().value()); +} + TEST(Rotation2dTest, RadiansToDegrees) { const Rotation2d rot1{units::radian_t{std::numbers::pi / 3.0}}; const Rotation2d rot2{units::radian_t{std::numbers::pi / 4.0}}; @@ -66,8 +76,8 @@ TEST(Rotation2dTest, Inequality) { TEST(Rotation2dTest, Constexpr) { constexpr Rotation2d defaultCtor; - constexpr Rotation2d radianCtor{5_rad}; - constexpr Rotation2d degreeCtor{270_deg}; + constexpr Rotation2d radianCtor{0.25_rad}; + constexpr Rotation2d degreeCtor{-90_deg}; constexpr Rotation2d rotation45{45_deg}; constexpr Rotation2d cartesianCtor{3.5, -3.5}; @@ -76,9 +86,9 @@ TEST(Rotation2dTest, Constexpr) { constexpr auto subtracted = cartesianCtor - degreeCtor; static_assert(defaultCtor.Radians() == 0_rad); - static_assert(degreeCtor.Degrees() == 270_deg); - static_assert(negated.Radians() == (-5_rad)); - static_assert(multiplied.Radians() == 10_rad); + static_assert(degreeCtor.Degrees() == -90_deg); + static_assert(negated.Radians() == -0.25_rad); + static_assert(multiplied.Radians() == 0.5_rad); static_assert(subtracted == rotation45); static_assert(radianCtor != degreeCtor); }