diff --git a/wpilibc/src/main/native/include/frc/interfaces/Gyro.h b/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
index 7e322c4313..b74a3cf5d6 100644
--- a/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
+++ b/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
@@ -66,6 +66,10 @@ 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 1d1c919d57..27377be03e 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,6 +52,10 @@ 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 8845ea5f9b..6caefa5d7b 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
@@ -15,15 +15,21 @@ import java.util.Objects;
/**
* A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
+ *
+ *
The angle is continuous, that is if a Rotation2d is constructed with 361 degrees, it will
+ * return 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in the
+ * rotations as it sweeps past from 360 to 0 on the second time around.
*/
@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;
}
@@ -35,7 +41,9 @@ public class Rotation2d implements Interpolatable {
*/
@JsonCreator
public Rotation2d(@JsonProperty(required = true, value = "radians") double value) {
- this(Math.cos(value), Math.sin(value));
+ m_value = value;
+ m_cos = Math.cos(value);
+ m_sin = Math.sin(value);
}
/**
@@ -53,12 +61,13 @@ public class Rotation2d implements Interpolatable {
m_sin = 0.0;
m_cos = 1.0;
}
+ m_value = Math.atan2(m_sin, m_cos);
}
/**
* Constructs and returns a Rotation2d with the given radian value.
*
- * @param radians The value of the angle in degrees.
+ * @param radians The value of the angle in radians.
* @return The rotation object with the desired angle value.
*/
public static Rotation2d fromRadians(double radians) {
@@ -118,7 +127,7 @@ public class Rotation2d implements Interpolatable {
* @return The inverse of the current rotation.
*/
public Rotation2d unaryMinus() {
- return new Rotation2d(m_cos, -m_sin);
+ return new Rotation2d(-m_value);
}
/**
@@ -128,7 +137,7 @@ public class Rotation2d implements Interpolatable {
* @return The new scaled Rotation2d.
*/
public Rotation2d times(double scalar) {
- return new Rotation2d(getRadians() * scalar);
+ return new Rotation2d(m_value * scalar);
}
/**
@@ -161,22 +170,22 @@ public class Rotation2d implements Interpolatable {
}
/**
- * Returns the radian value of the Rotation2d within (-pi, pi].
+ * Returns the radian value of the Rotation2d.
*
* @return The radian value of the Rotation2d.
*/
@JsonProperty
public double getRadians() {
- return Math.atan2(m_sin, m_cos);
+ return m_value;
}
/**
- * Returns the degree value of the Rotation2d within (-180, 180].
+ * Returns the degree value of the Rotation2d.
*
* @return The degree value of the Rotation2d.
*/
public double getDegrees() {
- return Math.toDegrees(getRadians());
+ return Math.toDegrees(m_value);
}
/**
@@ -185,7 +194,7 @@ public class Rotation2d implements Interpolatable {
* @return The number of rotations of the Rotation2d.
*/
public double getRotations() {
- return Units.radiansToRotations(getRadians());
+ return Units.radiansToRotations(m_value);
}
/**
@@ -217,7 +226,7 @@ public class Rotation2d implements Interpolatable {
@Override
public String toString() {
- return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", getRadians(), getDegrees());
+ return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
}
/**
@@ -237,7 +246,7 @@ public class Rotation2d implements Interpolatable {
@Override
public int hashCode() {
- return Objects.hash(getRadians());
+ return Objects.hash(m_value);
}
@Override
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
index be3315aa81..e40f3facaf 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
@@ -17,6 +17,11 @@ namespace frc {
/**
* A rotation in a 2D coordinate frame represented by a point on the unit circle
* (cosine and sine).
+ *
+ * The angle is continuous, that is if a Rotation2d is constructed with 361
+ * degrees, it will return 361 degrees. This allows algorithms that wouldn't
+ * want to see a discontinuity in the rotations as it sweeps past from 360 to 0
+ * on the second time around.
*/
class WPILIB_DLLEXPORT Rotation2d {
public:
@@ -124,18 +129,18 @@ class WPILIB_DLLEXPORT Rotation2d {
constexpr Rotation2d RotateBy(const Rotation2d& other) const;
/**
- * Returns the radian value of the rotation within (-pi, pi].
+ * Returns the radian value of the rotation.
*
* @return The radian value of the rotation.
*/
- constexpr units::radian_t Radians() const;
+ constexpr units::radian_t Radians() const { return m_value; }
/**
- * Returns the degree value of the rotation within (-180, 180].
+ * Returns the degree value of the rotation.
*
* @return The degree value of the rotation.
*/
- constexpr units::degree_t Degrees() const;
+ constexpr units::degree_t Degrees() const { return m_value; }
/**
* Returns the cosine of the rotation.
@@ -159,6 +164,7 @@ 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 9b5e45355d..eb31ebded6 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.inc
@@ -13,10 +13,11 @@
namespace frc {
constexpr Rotation2d::Rotation2d(units::radian_t value)
- : 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())) {}
+ : 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())) {}
constexpr Rotation2d::Rotation2d(units::degree_t value)
: Rotation2d(units::radian_t{value}) {}
@@ -30,14 +31,17 @@ 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_cos, -m_sin};
+ return Rotation2d{-m_value};
}
constexpr Rotation2d Rotation2d::operator*(double scalar) const {
- return Rotation2d(Radians() * scalar);
+ return Rotation2d(m_value * scalar);
}
constexpr Rotation2d Rotation2d::operator+(const Rotation2d& other) const {
@@ -63,14 +67,4 @@ 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 c6e75d4741..6702f1d186 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,15 +13,6 @@ 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);
@@ -80,7 +71,7 @@ class Rotation2dTest {
var rot = Rotation2d.fromDegrees(10.0);
assertEquals(30.0, rot.times(3.0).getDegrees(), kEpsilon);
- assertEquals(50.0, rot.times(41.0).getDegrees(), kEpsilon);
+ assertEquals(410.0, rot.times(41.0).getDegrees(), kEpsilon);
}
@Test
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
index 25cbecf9b6..4e2e6835c2 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
@@ -10,16 +10,6 @@
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}};
@@ -76,8 +66,8 @@ TEST(Rotation2dTest, Inequality) {
TEST(Rotation2dTest, Constexpr) {
constexpr Rotation2d defaultCtor;
- constexpr Rotation2d radianCtor{0.25_rad};
- constexpr Rotation2d degreeCtor{-90_deg};
+ constexpr Rotation2d radianCtor{5_rad};
+ constexpr Rotation2d degreeCtor{270_deg};
constexpr Rotation2d rotation45{45_deg};
constexpr Rotation2d cartesianCtor{3.5, -3.5};
@@ -86,9 +76,9 @@ TEST(Rotation2dTest, Constexpr) {
constexpr auto subtracted = cartesianCtor - degreeCtor;
static_assert(defaultCtor.Radians() == 0_rad);
- static_assert(degreeCtor.Degrees() == -90_deg);
- static_assert(negated.Radians() == -0.25_rad);
- static_assert(multiplied.Radians() == 0.5_rad);
+ static_assert(degreeCtor.Degrees() == 270_deg);
+ static_assert(negated.Radians() == -5_rad);
+ static_assert(multiplied.Radians() == 10_rad);
static_assert(subtracted == rotation45);
static_assert(radianCtor != degreeCtor);
}