diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index 6c98337171..09f25aa276 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -89,6 +89,74 @@ public class ChassisSpeeds { robotAngle); } + /** + * Adds two ChassisSpeeds and returns the sum. + * + *
For example, ChassisSpeeds{1.0, 0.5, 0.75} + ChassisSpeeds{2.0, 1.5, 0.25} = + * ChassisSpeeds{3.0, 2.0, 1.0} + * + * @param other The ChassisSpeeds to add. + * @return The sum of the ChassisSpeeds. + */ + public ChassisSpeeds plus(ChassisSpeeds other) { + return new ChassisSpeeds( + vxMetersPerSecond + other.vxMetersPerSecond, + vyMetersPerSecond + other.vyMetersPerSecond, + omegaRadiansPerSecond + other.omegaRadiansPerSecond); + } + + /** + * Subtracts the other ChassisSpeeds from the other ChassisSpeeds and returns the difference. + * + *
For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0} = + * ChassisSpeeds{4.0, 2.0, 1.0} + * + * @param other The ChassisSpeeds to subtract. + * @return The difference between the two ChassisSpeeds. + */ + public ChassisSpeeds minus(ChassisSpeeds other) { + return new ChassisSpeeds( + vxMetersPerSecond - other.vxMetersPerSecond, + vyMetersPerSecond - other.vyMetersPerSecond, + omegaRadiansPerSecond - other.omegaRadiansPerSecond); + } + + /** + * Returns the inverse of the current ChassisSpeeds. This is equivalent to negating all components + * of the ChassisSpeeds. + * + * @return The inverse of the current ChassisSpeeds. + */ + public ChassisSpeeds unaryMinus() { + return new ChassisSpeeds(-vxMetersPerSecond, -vyMetersPerSecond, -omegaRadiansPerSecond); + } + + /** + * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds. + * + *
For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2 = ChassisSpeeds{4.0, 5.0, 1.0} + * + * @param scalar The scalar to multiply by. + * @return The scaled ChassisSpeeds. + */ + public ChassisSpeeds times(double scalar) { + return new ChassisSpeeds( + vxMetersPerSecond * scalar, vyMetersPerSecond * scalar, omegaRadiansPerSecond * scalar); + } + + /** + * Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds. + * + *
For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2 = ChassisSpeeds{1.0, 1.25, 0.5} + * + * @param scalar The scalar to multiply by. + * @return The reference to the new mutated object. + */ + public ChassisSpeeds div(double scalar) { + return new ChassisSpeeds( + vxMetersPerSecond / scalar, vyMetersPerSecond / scalar, omegaRadiansPerSecond / scalar); + } + @Override public String toString() { return String.format( diff --git a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h index 37fe768911..905bb0a3b7 100644 --- a/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h +++ b/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h @@ -81,5 +81,70 @@ struct WPILIB_DLLEXPORT ChassisSpeeds { fieldRelativeSpeeds.vy, fieldRelativeSpeeds.omega, robotAngle); } + + /** + * Adds two ChassisSpeeds and returns the sum. + * + *
For example, ChassisSpeeds{1.0, 0.5, 1.5} + ChassisSpeeds{2.0, 1.5, 0.5} + * = ChassisSpeeds{3.0, 2.0, 2.0} + * + * @param other The ChassisSpeeds to add. + * + * @return The sum of the ChassisSpeeds. + */ + constexpr ChassisSpeeds operator+(const ChassisSpeeds& other) const { + return {vx + other.vx, vy + other.vy, omega + other.omega}; + } + + /** + * Subtracts the other ChassisSpeeds from the other ChassisSpeeds and returns + * the difference. + * + *
For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0} + * = ChassisSpeeds{4.0, 2.0, 1.0} + * + * @param other The ChassisSpeeds to subtract. + * + * @return The difference between the two ChassisSpeeds. + */ + constexpr ChassisSpeeds operator-(const ChassisSpeeds& other) const { + return *this + -other; + } + + /** + * Returns the inverse of the current ChassisSpeeds. + * This is equivalent to negating all components of the ChassisSpeeds. + * + * @return The inverse of the current ChassisSpeeds. + */ + constexpr ChassisSpeeds operator-() const { return {-vx, -vy, -omega}; } + + /** + * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds. + * + *
For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2 + * = ChassisSpeeds{4.0, 5.0, 1.0} + * + * @param scalar The scalar to multiply by. + * + * @return The scaled ChassisSpeeds. + */ + constexpr ChassisSpeeds operator*(double scalar) const { + return {scalar * vx, scalar * vy, scalar * omega}; + } + + /** + * Divides the ChassisSpeeds by a scalar and returns the new ChassisSpeeds. + * + *
For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2 + * = ChassisSpeeds{1.0, 1.25, 0.5} + * + * @param scalar The scalar to multiply by. + * + * @return The reference to the new mutated object. + */ + constexpr ChassisSpeeds operator/(double scalar) const { + return operator*(1.0 / scalar); + } }; } // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java index b9c3785caa..f572ac8cc5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java @@ -23,4 +23,60 @@ class ChassisSpeedsTest { () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon), () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)); } + + @Test + void testPlus() { + final var left = new ChassisSpeeds(1.0, 0.5, 0.75); + final var right = new ChassisSpeeds(2.0, 1.5, 0.25); + + final var chassisSpeeds = left.plus(right); + + assertAll( + () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond), + () -> assertEquals(2.0, chassisSpeeds.vyMetersPerSecond), + () -> assertEquals(1.0, chassisSpeeds.omegaRadiansPerSecond)); + } + + @Test + void testMinus() { + final var left = new ChassisSpeeds(1.0, 0.5, 0.75); + final var right = new ChassisSpeeds(2.0, 0.5, 0.25); + + final var chassisSpeeds = left.minus(right); + + assertAll( + () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond), + () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond), + () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond)); + } + + @Test + void testUnaryMinus() { + final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).unaryMinus(); + + assertAll( + () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond), + () -> assertEquals(-0.5, chassisSpeeds.vyMetersPerSecond), + () -> assertEquals(-0.75, chassisSpeeds.omegaRadiansPerSecond)); + } + + @Test + void testMultiplication() { + final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).times(2.0); + + assertAll( + () -> assertEquals(2.0, chassisSpeeds.vxMetersPerSecond), + () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond), + () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond)); + } + + @Test + void testDivision() { + final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).div(2.0); + + assertAll( + () -> assertEquals(0.5, chassisSpeeds.vxMetersPerSecond), + () -> assertEquals(0.25, chassisSpeeds.vyMetersPerSecond), + () -> assertEquals(0.375, chassisSpeeds.omegaRadiansPerSecond)); + } } diff --git a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp index 4e0b3e5bf7..42f1305d2a 100644 --- a/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp @@ -15,3 +15,55 @@ TEST(ChassisSpeedsTest, FieldRelativeConstruction) { EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon); EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon); } + +TEST(ChassisSpeedsTest, Plus) { + const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s}; + const frc::ChassisSpeeds right{2.0_mps, 1.5_mps, 0.25_rad_per_s}; + + const frc::ChassisSpeeds result = left + right; + + EXPECT_NEAR(3.0, result.vx.value(), kEpsilon); + EXPECT_NEAR(2.0, result.vy.value(), kEpsilon); + EXPECT_NEAR(1.0, result.omega.value(), kEpsilon); +} + +TEST(ChassisSpeedsTest, Minus) { + const frc::ChassisSpeeds left{1.0_mps, 0.5_mps, 0.75_rad_per_s}; + const frc::ChassisSpeeds right{2.0_mps, 0.5_mps, 0.25_rad_per_s}; + + const frc::ChassisSpeeds result = left - right; + + EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon); + EXPECT_NEAR(0, result.vy.value(), kEpsilon); + EXPECT_NEAR(0.5, result.omega.value(), kEpsilon); +} + +TEST(ChassisSpeedsTest, UnaryMinus) { + const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s}; + + const frc::ChassisSpeeds result = -speeds; + + EXPECT_NEAR(-1.0, result.vx.value(), kEpsilon); + EXPECT_NEAR(-0.5, result.vy.value(), kEpsilon); + EXPECT_NEAR(-0.75, result.omega.value(), kEpsilon); +} + +TEST(ChassisSpeedsTest, Multiplication) { + const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s}; + + const frc::ChassisSpeeds result = speeds * 2; + + EXPECT_NEAR(2.0, result.vx.value(), kEpsilon); + EXPECT_NEAR(1.0, result.vy.value(), kEpsilon); + EXPECT_NEAR(1.5, result.omega.value(), kEpsilon); +} + +TEST(ChassisSpeedsTest, Division) { + const frc::ChassisSpeeds speeds{1.0_mps, 0.5_mps, 0.75_rad_per_s}; + + const frc::ChassisSpeeds result = speeds / 2; + + EXPECT_NEAR(0.5, result.vx.value(), kEpsilon); + EXPECT_NEAR(0.25, result.vy.value(), kEpsilon); + EXPECT_NEAR(0.375, result.omega.value(), kEpsilon); +}