diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp index 85232193a4..4a65d11c8e 100644 --- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp @@ -115,8 +115,8 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::ArcadeDriveIK( // Square the inputs (while preserving the sign) to increase fine control // while permitting full power. if (squareInputs) { - xSpeed = std::copysign(xSpeed * xSpeed, xSpeed); - zRotation = std::copysign(zRotation * zRotation, zRotation); + xSpeed = CopySignPow(xSpeed, 2); + zRotation = CopySignPow(zRotation, 2); } double leftSpeed = xSpeed - zRotation; @@ -170,8 +170,8 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::TankDriveIK( // Square the inputs (while preserving the sign) to increase fine control // while permitting full power. if (squareInputs) { - leftSpeed = std::copysign(leftSpeed * leftSpeed, leftSpeed); - rightSpeed = std::copysign(rightSpeed * rightSpeed, rightSpeed); + leftSpeed = CopySignPow(leftSpeed, 2); + rightSpeed = CopySignPow(rightSpeed, 2); } return {leftSpeed, rightSpeed}; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java index 40e34a803d..676a8ea470 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java @@ -265,8 +265,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC // Square the inputs (while preserving the sign) to increase fine control // while permitting full power. if (squareInputs) { - xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed); - zRotation = Math.copySign(zRotation * zRotation, zRotation); + xSpeed = MathUtil.copySignPow(xSpeed, 2); + zRotation = MathUtil.copySignPow(zRotation, 2); } double leftSpeed = xSpeed - zRotation; @@ -340,8 +340,8 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC // Square the inputs (while preserving the sign) to increase fine control // while permitting full power. if (squareInputs) { - leftSpeed = Math.copySign(leftSpeed * leftSpeed, leftSpeed); - rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed); + leftSpeed = MathUtil.copySignPow(leftSpeed, 2); + rightSpeed = MathUtil.copySignPow(rightSpeed, 2); } return new WheelSpeeds(leftSpeed, rightSpeed); diff --git a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java index 1e374ec583..239a6a31fb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java @@ -107,6 +107,42 @@ public final class MathUtil { return applyDeadband(value, deadband, 1); } + /** + * Raises the input to the power of the given exponent while preserving its sign. + * + *

The function normalizes the input value to the range [0, 1] based on the maximum magnitude, + * raises it to the power of the exponent, then scales the result back to the original range and + * copying the sign. This keeps the value in the original range and gives consistent curve + * behavior regardless of the input value's scale. + * + *

This is useful for applying smoother or more aggressive control response curves (e.g. + * joystick input shaping). + * + * @param value The input value to transform. + * @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be + * positive. + * @param maxMagnitude The maximum expected absolute value of input. Must be positive. + * @return The transformed value with the same sign and scaled to the input range. + */ + public static double copySignPow(double value, double exponent, double maxMagnitude) { + return Math.copySign(Math.pow(Math.abs(value) / maxMagnitude, exponent), value) * maxMagnitude; + } + + /** + * Raises the input to the power of the given exponent while preserving its sign. + * + *

This is useful for applying smoother or more aggressive control response curves (e.g. + * joystick input shaping). + * + * @param value The input value to transform. + * @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared curve). Must be + * positive. + * @return The transformed value with the same sign. + */ + public static double copySignPow(double value, double exponent) { + return copySignPow(value, exponent, 1); + } + /** * Returns modulus of input. * diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index 060be0da91..97705811f9 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -94,6 +94,42 @@ constexpr T ApplyDeadband(T value, T deadband, T maxMagnitude = T{1.0}) { } } +/** + * Raises the input to the power of the given exponent while preserving its + * sign. + * + * The function normalizes the input value to the range [0, 1] based on the + * maximum magnitude, raises it to the power of the exponent, then scales the + * result back to the original range and copying the sign. This keeps the value + * in the original range and gives consistent curve behavior regardless of the + * input value's scale. + * + * This is useful for applying smoother or more aggressive control response + * curves (e.g. joystick input shaping). + * + * @param value The input value to transform. + * @param exponent The exponent to apply (e.g. 1.0 = linear, 2.0 = squared + * curve). Must be positive. + * @param maxMagnitude The maximum expected absolute value of input. Must be + * positive. + * @return The transformed value with the same sign and scaled to the input + * range. + */ +template + requires std::is_arithmetic_v || units::traits::is_unit_t_v +constexpr T CopySignPow(T value, double exponent, T maxMagnitude = T{1.0}) { + if constexpr (std::is_arithmetic_v) { + return gcem::copysign( + gcem::pow(gcem::abs(value) / maxMagnitude, exponent) * maxMagnitude, + value); + } else { + return units::math::copysign( + gcem::pow((units::math::abs(value) / maxMagnitude).value(), exponent) * + maxMagnitude, + value); + } +} + /** * Returns modulus of input. * diff --git a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java index 93ef1ef734..1fcef74d1f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java @@ -72,6 +72,44 @@ class MathUtilTest extends UtilityClassTest { assertEquals(80.0, MathUtil.applyDeadband(100.0, 20, Double.POSITIVE_INFINITY)); } + @Test + void testCopySignPow() { + assertEquals(0.5, MathUtil.copySignPow(0.5, 1.0)); + assertEquals(-0.5, MathUtil.copySignPow(-0.5, 1.0)); + + assertEquals(0.5 * 0.5, MathUtil.copySignPow(0.5, 2.0)); + assertEquals(-(0.5 * 0.5), MathUtil.copySignPow(-0.5, 2.0)); + + assertEquals(Math.sqrt(0.5), MathUtil.copySignPow(0.5, 0.5)); + assertEquals(-Math.sqrt(0.5), MathUtil.copySignPow(-0.5, 0.5)); + + assertEquals(0.0, MathUtil.copySignPow(0.0, 2.0)); + assertEquals(1.0, MathUtil.copySignPow(1.0, 2.0)); + assertEquals(-1.0, MathUtil.copySignPow(-1.0, 2.0)); + + assertEquals(Math.pow(0.8, 0.3), MathUtil.copySignPow(0.8, 0.3)); + assertEquals(-Math.pow(0.8, 0.3), MathUtil.copySignPow(-0.8, 0.3)); + } + + @Test + void testCopySignPowMaxMagnitude() { + assertEquals(5, MathUtil.copySignPow(5.0, 1.0, 10.0)); + assertEquals(-5, MathUtil.copySignPow(-5.0, 1.0, 10.0)); + + assertEquals(0.5 * 0.5 * 10, MathUtil.copySignPow(5.0, 2.0, 10.0)); + assertEquals(-0.5 * 0.5 * 10, MathUtil.copySignPow(-5.0, 2.0, 10.0)); + + assertEquals(Math.sqrt(0.5) * 10, MathUtil.copySignPow(5.0, 0.5, 10.0)); + assertEquals(-Math.sqrt(0.5) * 10, MathUtil.copySignPow(-5.0, 0.5, 10.0)); + + assertEquals(0.0, MathUtil.copySignPow(0.0, 2.0, 5.0)); + assertEquals(5.0, MathUtil.copySignPow(5.0, 2.0, 5.0)); + assertEquals(-5.0, MathUtil.copySignPow(-5.0, 2.0, 5.0)); + + assertEquals(Math.pow(0.8, 0.3) * 100, MathUtil.copySignPow(80, 0.3, 100.0)); + assertEquals(-Math.pow(0.8, 0.3) * 100, MathUtil.copySignPow(-80, 0.3, 100.0)); + } + @Test void testInputModulus() { // These tests check error wrapping. That is, the result of wrapping the diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp index fc9405ce38..1479c0d18f 100644 --- a/wpimath/src/test/native/cpp/MathUtilTest.cpp +++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp @@ -65,6 +65,61 @@ TEST(MathUtilTest, ApplyDeadbandLargeMaxMagnitude) { frc::ApplyDeadband(100.0, 20.0, std::numeric_limits::infinity())); } +TEST(MathUtilTest, CopySignPow) { + EXPECT_DOUBLE_EQ(0.5, frc::CopySignPow(0.5, 1.0)); + EXPECT_DOUBLE_EQ(-0.5, frc::CopySignPow(-0.5, 1.0)); + + EXPECT_DOUBLE_EQ(0.5 * 0.5, frc::CopySignPow(0.5, 2.0)); + EXPECT_DOUBLE_EQ(-(0.5 * 0.5), frc::CopySignPow(-0.5, 2.0)); + + EXPECT_DOUBLE_EQ(std::sqrt(0.5), frc::CopySignPow(0.5, 0.5)); + EXPECT_DOUBLE_EQ(-std::sqrt(0.5), frc::CopySignPow(-0.5, 0.5)); + + EXPECT_DOUBLE_EQ(0.0, frc::CopySignPow(0.0, 2.0)); + EXPECT_DOUBLE_EQ(1.0, frc::CopySignPow(1.0, 2.0)); + EXPECT_DOUBLE_EQ(-1.0, frc::CopySignPow(-1.0, 2.0)); + + EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3), frc::CopySignPow(0.8, 0.3)); + EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3), frc::CopySignPow(-0.8, 0.3)); +} + +TEST(MathUtilTest, CopySignPowWithMaxMagnitude) { + EXPECT_DOUBLE_EQ(5.0, frc::CopySignPow(5.0, 1.0, 10.0)); + EXPECT_DOUBLE_EQ(-5.0, frc::CopySignPow(-5.0, 1.0, 10.0)); + + EXPECT_DOUBLE_EQ(0.5 * 0.5 * 10, frc::CopySignPow(5.0, 2.0, 10.0)); + EXPECT_DOUBLE_EQ(-0.5 * 0.5 * 10, frc::CopySignPow(-5.0, 2.0, 10.0)); + + EXPECT_DOUBLE_EQ(std::sqrt(0.5) * 10, frc::CopySignPow(5.0, 0.5, 10.0)); + EXPECT_DOUBLE_EQ(-std::sqrt(0.5) * 10, frc::CopySignPow(-5.0, 0.5, 10.0)); + + EXPECT_DOUBLE_EQ(0.0, frc::CopySignPow(0.0, 2.0, 5.0)); + EXPECT_DOUBLE_EQ(5.0, frc::CopySignPow(5.0, 2.0, 5.0)); + EXPECT_DOUBLE_EQ(-5.0, frc::CopySignPow(-5.0, 2.0, 5.0)); + + EXPECT_DOUBLE_EQ(std::pow(0.8, 0.3) * 100, + frc::CopySignPow(80.0, 0.3, 100.0)); + EXPECT_DOUBLE_EQ(-std::pow(0.8, 0.3) * 100, + frc::CopySignPow(-80.0, 0.3, 100.0)); +} + +TEST(MathUtilTest, CopySignPowWithUnits) { + EXPECT_DOUBLE_EQ( + 0, frc::CopySignPow(0_mps, 2.0).value()); + EXPECT_DOUBLE_EQ( + 1, frc::CopySignPow(1_mps, 2.0).value()); + EXPECT_DOUBLE_EQ( + -1, frc::CopySignPow(-1_mps, 2.0).value()); + + EXPECT_DOUBLE_EQ( + 0.5 * 0.5 * 10, + frc::CopySignPow(5_mps, 2.0, 10_mps).value()); + EXPECT_DOUBLE_EQ( + -0.5 * 0.5 * 10, + frc::CopySignPow(-5_mps, 2.0, 10_mps) + .value()); +} + TEST(MathUtilTest, InputModulus) { // These tests check error wrapping. That is, the result of wrapping the // result of an angle reference minus the measurement.