From a238cec12bbce3adbe60fe88e36cc0dc27c778ba Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 26 May 2021 00:09:36 -0700 Subject: [PATCH] [wpiutil] Deprecate wpi::math constants in favor of wpi::numbers (#3383) The constants were moved from std::math to std::numbers before ratification in C++20. --- glass/src/lib/native/cpp/hardware/Gyro.cpp | 6 +- glass/src/lib/native/cpp/other/Drive.cpp | 19 ++--- .../command/MecanumControllerCommandTest.cpp | 2 +- .../command/SwerveControllerCommandTest.cpp | 2 +- wpilibc/src/main/native/cpp/Joystick.cpp | 4 +- .../main/native/cpp/drive/KilloughDrive.cpp | 18 ++--- .../main/native/cpp/drive/MecanumDrive.cpp | 6 +- .../src/main/native/cpp/drive/Vector2d.cpp | 6 +- .../HolonomicDriveControllerTest.cpp | 5 +- .../controller/ProfiledPIDInputOutputTest.cpp | 18 ++--- .../cpp/controller/RamseteControllerTest.cpp | 3 +- .../cpp/simulation/AnalogEncoderSimTest.cpp | 4 +- .../simulation/SingleJointedArmSimTest.cpp | 4 +- .../cpp/examples/ArmBot/include/Constants.h | 8 +- .../ArmBotOffboard/include/Constants.h | 8 +- .../cpp/examples/ArmSimulation/cpp/Robot.cpp | 4 +- .../DifferentialDriveBot/include/Drivetrain.h | 8 +- .../include/Drivetrain.h | 8 +- .../DriveDistanceOffboard/include/Constants.h | 2 +- .../ElevatorProfiledPID/cpp/Robot.cpp | 4 +- .../examples/ElevatorSimulation/cpp/Robot.cpp | 4 +- .../ElevatorTrapezoidProfile/cpp/Robot.cpp | 2 +- .../main/cpp/examples/Encoder/cpp/Robot.cpp | 4 +- .../examples/Frisbeebot/include/Constants.h | 5 +- .../GearsBot/cpp/subsystems/DriveTrain.cpp | 6 +- .../GyroDriveCommands/include/Constants.h | 5 +- .../HatchbotInlined/include/Constants.h | 5 +- .../HatchbotTraditional/include/Constants.h | 5 +- .../examples/MecanumBot/include/Drivetrain.h | 4 +- .../include/Constants.h | 5 +- .../include/Drivetrain.h | 4 +- .../MotorControlEncoder/cpp/Robot.cpp | 4 +- .../PacGoat/cpp/subsystems/DriveTrain.cpp | 6 +- .../RamseteCommand/include/Constants.h | 5 +- .../RamseteController/include/Drivetrain.h | 8 +- .../cpp/commands/TurnDegrees.cpp | 4 +- .../cpp/subsystems/Drivetrain.cpp | 6 +- .../include/Drivetrain.h | 8 +- .../cpp/examples/StateSpaceArm/cpp/Robot.cpp | 7 +- .../include/Constants.h | 4 +- .../examples/StateSpaceElevator/cpp/Robot.cpp | 4 +- .../examples/StateSpaceFlywheel/cpp/Robot.cpp | 4 +- .../StateSpaceFlywheelSysId/cpp/Robot.cpp | 4 +- .../examples/SwerveBot/cpp/SwerveModule.cpp | 13 ++-- .../examples/SwerveBot/include/Drivetrain.h | 4 +- .../examples/SwerveBot/include/SwerveModule.h | 6 +- .../cpp/RobotContainer.cpp | 4 +- .../cpp/subsystems/SwerveModule.cpp | 8 +- .../include/Constants.h | 7 +- .../include/subsystems/SwerveModule.h | 6 +- .../cpp/SwerveModule.cpp | 13 ++-- .../include/Drivetrain.h | 4 +- .../include/SwerveModule.h | 6 +- .../examples/swervebot/SwerveModule.java | 4 +- .../subsystems/SwerveModule.java | 4 +- .../SwerveModule.java | 4 +- wpimath/src/dev/native/cpp/main.cpp | 4 +- .../src/main/native/include/frc/MathUtil.h | 7 +- .../include/frc/estimator/AngleStatistics.h | 4 +- wpimath/src/main/native/include/units/math.h | 2 - .../test/native/cpp/LinearFilterNoiseTest.cpp | 4 +- .../native/cpp/LinearFilterOutputTest.cpp | 6 +- wpimath/src/test/native/cpp/MathUtilTest.cpp | 27 +++---- .../cpp/estimator/AngleStatisticsTest.cpp | 16 ++-- .../native/cpp/geometry/Rotation2dTest.cpp | 12 +-- .../test/native/cpp/geometry/Twist2dTest.cpp | 10 +-- .../DifferentialDriveKinematicsTest.cpp | 18 +++-- .../DifferentialDriveOdometryTest.cpp | 4 +- .../kinematics/MecanumDriveKinematicsTest.cpp | 6 +- .../kinematics/SwerveDriveKinematicsTest.cpp | 10 +-- wpiutil/.styleguide | 2 + wpiutil/src/main/native/include/wpi/math | 75 ++++--------------- wpiutil/src/main/native/include/wpi/numbers | 64 ++++++++++++++++ 73 files changed, 322 insertions(+), 284 deletions(-) create mode 100644 wpiutil/src/main/native/include/wpi/numbers diff --git a/glass/src/lib/native/cpp/hardware/Gyro.cpp b/glass/src/lib/native/cpp/hardware/Gyro.cpp index 67297e5566..36a3525d66 100644 --- a/glass/src/lib/native/cpp/hardware/Gyro.cpp +++ b/glass/src/lib/native/cpp/hardware/Gyro.cpp @@ -10,7 +10,7 @@ #include #include -#include +#include #include "glass/Context.h" #include "glass/DataSource.h" @@ -54,7 +54,7 @@ void glass::DisplayGyro(GyroModel* m) { // Draw the spokes at every 5 degrees and a "major" spoke every 45 degrees. for (int i = -175; i <= 180; i += 5) { - double radians = i * 2 * wpi::math::pi / 360.0; + double radians = i * 2 * wpi::numbers::pi / 360.0; ImVec2 direction(std::sin(radians), -std::cos(radians)); bool major = i % 45 == 0; @@ -74,7 +74,7 @@ void glass::DisplayGyro(GyroModel* m) { draw->AddCircleFilled(center, radius * 0.075, secondaryColor, 50); - double radians = value * 2 * wpi::math::pi / 360.0; + double radians = value * 2 * wpi::numbers::pi / 360.0; draw->AddLine( center - ImVec2(1, 0), center + ImVec2(std::sin(radians), -std::cos(radians)) * radius * 0.95f, diff --git a/glass/src/lib/native/cpp/other/Drive.cpp b/glass/src/lib/native/cpp/other/Drive.cpp index 39357d4ad6..9dc1675088 100644 --- a/glass/src/lib/native/cpp/other/Drive.cpp +++ b/glass/src/lib/native/cpp/other/Drive.cpp @@ -11,7 +11,7 @@ #include #include -#include +#include #include "glass/Context.h" #include "glass/DataSource.h" @@ -55,11 +55,11 @@ void glass::DisplayDrive(DriveModel* m) { draw->AddTriangleFilled( arrowPos, arrowPos + ImRotate(ImVec2(0.0f, 7.5f), - std::cos(angle + wpi::math::pi / 4), - std::sin(angle + wpi::math::pi / 4)), + std::cos(angle + wpi::numbers::pi / 4), + std::sin(angle + wpi::numbers::pi / 4)), arrowPos + ImRotate(ImVec2(0.0f, 7.5f), - std::cos(angle - wpi::math::pi / 4), - std::sin(angle - wpi::math::pi / 4)), + std::cos(angle - wpi::numbers::pi / 4), + std::sin(angle - wpi::numbers::pi / 4)), color); }; @@ -88,20 +88,21 @@ void glass::DisplayDrive(DriveModel* m) { if (rotation != 0) { float radius = 60.0f; double a1 = 0.0; - double a2 = wpi::math::pi / 2 * rotation; + double a2 = wpi::numbers::pi / 2 * rotation; draw->PathArcTo(center, radius, a1, a2, 20); draw->PathStroke(color, false); - draw->PathArcTo(center, radius, a1 + wpi::math::pi, a2 + wpi::math::pi, 20); + draw->PathArcTo(center, radius, a1 + wpi::numbers::pi, + a2 + wpi::numbers::pi, 20); draw->PathStroke(color, false); - double adder = rotation < 0 ? wpi::math::pi : 0; + double adder = rotation < 0 ? wpi::numbers::pi : 0; auto arrowPos = center + ImVec2(radius * -std::cos(a2), radius * -std::sin(a2)); drawArrow(arrowPos, a2 + adder); - a2 += wpi::math::pi; + a2 += wpi::numbers::pi; arrowPos = center + ImVec2(radius * -std::cos(a2), radius * -std::sin(a2)); drawArrow(arrowPos, a2 + adder); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp index d9094ea129..e72a41ab38 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp @@ -14,7 +14,7 @@ #include #include #include -#include +#include #include "gtest/gtest.h" diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp index 357dd80be4..60546f63a3 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include "gtest/gtest.h" diff --git a/wpilibc/src/main/native/cpp/Joystick.cpp b/wpilibc/src/main/native/cpp/Joystick.cpp index 71e41f9412..3ca2ff0b5c 100644 --- a/wpilibc/src/main/native/cpp/Joystick.cpp +++ b/wpilibc/src/main/native/cpp/Joystick.cpp @@ -7,7 +7,7 @@ #include #include -#include +#include using namespace frc; @@ -114,5 +114,5 @@ double Joystick::GetDirectionRadians() const { } double Joystick::GetDirectionDegrees() const { - return (180 / wpi::math::pi) * GetDirectionRadians(); + return (180 / wpi::numbers::pi) * GetDirectionRadians(); } diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp index 7863eab91b..e2b55caf11 100644 --- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp @@ -8,7 +8,7 @@ #include #include -#include +#include #include "frc/SpeedController.h" #include "frc/smartdashboard/SendableBuilder.h" @@ -37,12 +37,12 @@ KilloughDrive::KilloughDrive(SpeedController& leftMotor, : m_leftMotor(&leftMotor), m_rightMotor(&rightMotor), m_backMotor(&backMotor) { - m_leftVec = {std::cos(leftMotorAngle * (wpi::math::pi / 180.0)), - std::sin(leftMotorAngle * (wpi::math::pi / 180.0))}; - m_rightVec = {std::cos(rightMotorAngle * (wpi::math::pi / 180.0)), - std::sin(rightMotorAngle * (wpi::math::pi / 180.0))}; - m_backVec = {std::cos(backMotorAngle * (wpi::math::pi / 180.0)), - std::sin(backMotorAngle * (wpi::math::pi / 180.0))}; + m_leftVec = {std::cos(leftMotorAngle * (wpi::numbers::pi / 180.0)), + std::sin(leftMotorAngle * (wpi::numbers::pi / 180.0))}; + m_rightVec = {std::cos(rightMotorAngle * (wpi::numbers::pi / 180.0)), + std::sin(rightMotorAngle * (wpi::numbers::pi / 180.0))}; + m_backVec = {std::cos(backMotorAngle * (wpi::numbers::pi / 180.0)), + std::sin(backMotorAngle * (wpi::numbers::pi / 180.0))}; auto& registry = SendableRegistry::GetInstance(); registry.AddChild(this, m_leftMotor); registry.AddChild(this, m_rightMotor); @@ -81,8 +81,8 @@ void KilloughDrive::DrivePolar(double magnitude, double angle, reported = true; } - DriveCartesian(magnitude * std::sin(angle * (wpi::math::pi / 180.0)), - magnitude * std::cos(angle * (wpi::math::pi / 180.0)), + DriveCartesian(magnitude * std::sin(angle * (wpi::numbers::pi / 180.0)), + magnitude * std::cos(angle * (wpi::numbers::pi / 180.0)), zRotation, 0.0); } diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp index 3f4d2c9ea9..c1012557c3 100644 --- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp +++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp @@ -8,7 +8,7 @@ #include #include -#include +#include #include "frc/SpeedController.h" #include "frc/drive/Vector2d.h" @@ -73,8 +73,8 @@ void MecanumDrive::DrivePolar(double magnitude, double angle, reported = true; } - DriveCartesian(magnitude * std::cos(angle * (wpi::math::pi / 180.0)), - magnitude * std::sin(angle * (wpi::math::pi / 180.0)), + DriveCartesian(magnitude * std::cos(angle * (wpi::numbers::pi / 180.0)), + magnitude * std::sin(angle * (wpi::numbers::pi / 180.0)), zRotation, 0.0); } diff --git a/wpilibc/src/main/native/cpp/drive/Vector2d.cpp b/wpilibc/src/main/native/cpp/drive/Vector2d.cpp index 58ad6fc5b4..a342dc276d 100644 --- a/wpilibc/src/main/native/cpp/drive/Vector2d.cpp +++ b/wpilibc/src/main/native/cpp/drive/Vector2d.cpp @@ -6,7 +6,7 @@ #include -#include +#include using namespace frc; @@ -16,8 +16,8 @@ Vector2d::Vector2d(double x, double y) { } void Vector2d::Rotate(double angle) { - double cosA = std::cos(angle * (wpi::math::pi / 180.0)); - double sinA = std::sin(angle * (wpi::math::pi / 180.0)); + double cosA = std::cos(angle * (wpi::numbers::pi / 180.0)); + double sinA = std::sin(angle * (wpi::numbers::pi / 180.0)); double out[2]; out[0] = x * cosA - y * sinA; out[1] = x * sinA + y * cosA; diff --git a/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp index 388c346c31..0c3a0b6a24 100644 --- a/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp @@ -4,7 +4,7 @@ #include #include -#include +#include #include "frc/MathUtil.h" #include "frc/controller/HolonomicDriveController.h" @@ -15,7 +15,8 @@ EXPECT_LE(units::math::abs(val1 - val2), eps) static constexpr units::meter_t kTolerance{1 / 12.0}; -static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0}; +static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi / + 180.0}; TEST(HolonomicDriveControllerTest, ReachesReference) { frc::HolonomicDriveController controller{ diff --git a/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp index 10de3b9274..24e3daf61f 100644 --- a/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include "frc/controller/ProfiledPIDController.h" #include "gtest/gtest.h" @@ -40,8 +40,8 @@ TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest1) { TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest2) { controller->SetP(1); - controller->EnableContinuousInput(-units::radian_t{wpi::math::pi}, - units::radian_t{wpi::math::pi}); + controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi}, + units::radian_t{wpi::numbers::pi}); static constexpr units::radian_t kSetpoint{-3.4826633343199735}; static constexpr units::radian_t kMeasurement{-3.1352207333939606}; @@ -52,13 +52,13 @@ TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest2) { // Error must be less than half the input range at all times EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement), - units::radian_t{wpi::math::pi}); + units::radian_t{wpi::numbers::pi}); } TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest3) { controller->SetP(1); - controller->EnableContinuousInput(-units::radian_t{wpi::math::pi}, - units::radian_t{wpi::math::pi}); + controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi}, + units::radian_t{wpi::numbers::pi}); static constexpr units::radian_t kSetpoint{-3.5176604690006377}; static constexpr units::radian_t kMeasurement{3.1191729343822456}; @@ -69,13 +69,13 @@ TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest3) { // Error must be less than half the input range at all times EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement), - units::radian_t{wpi::math::pi}); + units::radian_t{wpi::numbers::pi}); } TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest4) { controller->SetP(1); controller->EnableContinuousInput(0_rad, - units::radian_t{2.0 * wpi::math::pi}); + units::radian_t{2.0 * wpi::numbers::pi}); static constexpr units::radian_t kSetpoint{2.78}; static constexpr units::radian_t kMeasurement{3.12}; @@ -86,7 +86,7 @@ TEST_F(ProfiledPIDInputOutputTest, ContinuousInputTest4) { // Error must be less than half the input range at all times EXPECT_LT(units::math::abs(controller->GetSetpoint().position - kMeasurement), - units::radian_t{wpi::math::pi}); + units::radian_t{wpi::numbers::pi}); } TEST_F(ProfiledPIDInputOutputTest, ProportionalGainOutputTest) { diff --git a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp index e0c5100d8d..101dea2b87 100644 --- a/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp +++ b/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp @@ -13,7 +13,8 @@ EXPECT_LE(units::math::abs(val1 - val2), eps) static constexpr units::meter_t kTolerance{1 / 12.0}; -static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0}; +static constexpr units::radian_t kAngularTolerance{2.0 * wpi::numbers::pi / + 180.0}; TEST(RamseteControllerTest, ReachesReference) { frc::RamseteController controller{2.0, 0.7}; diff --git a/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp index 6e3372acc2..9c51f3e201 100644 --- a/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp @@ -4,7 +4,7 @@ #include #include -#include +#include #include "frc/AnalogEncoder.h" #include "frc/AnalogInput.h" @@ -22,6 +22,6 @@ TEST(AnalogEncoderSimTest, TestBasic) { 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, + EXPECT_NEAR(encoderSim.GetPosition().Radians().to(), wpi::numbers::pi, 1E-8); } diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp index 2c4606ad93..f682fe7867 100644 --- a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include +#include #include "frc/simulation/SingleJointedArmSim.h" #include "gtest/gtest.h" @@ -18,5 +18,5 @@ TEST(SingleJointedArmTest, Disabled) { } // The arm should swing down. - EXPECT_NEAR(sim.GetAngle().to(), -wpi::math::pi / 2, 0.01); + EXPECT_NEAR(sim.GetAngle().to(), -wpi::numbers::pi / 2, 0.01); } diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h index 87cfc72d6c..932875c68f 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h @@ -8,7 +8,7 @@ #include #include #include -#include +#include /** * The Constants header provides a convenient place for teams to hold robot-wide @@ -34,7 +34,8 @@ constexpr int kEncoderCPR = 1024; constexpr double kWheelDiameterInches = 6; constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterInches * wpi::math::pi) / static_cast(kEncoderCPR); + (kWheelDiameterInches * wpi::numbers::pi) / + static_cast(kEncoderCPR); } // namespace DriveConstants namespace ArmConstants { @@ -54,7 +55,8 @@ constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s); constexpr int kEncoderPorts[]{4, 5}; constexpr int kEncoderPPR = 256; -constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR; +constexpr auto kEncoderDistancePerPulse = + 2.0_rad * wpi::numbers::pi / kEncoderPPR; // The offset of the arm from the horizontal in its neutral position, // measured from the horizontal diff --git a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h index 87cfc72d6c..932875c68f 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h @@ -8,7 +8,7 @@ #include #include #include -#include +#include /** * The Constants header provides a convenient place for teams to hold robot-wide @@ -34,7 +34,8 @@ constexpr int kEncoderCPR = 1024; constexpr double kWheelDiameterInches = 6; constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterInches * wpi::math::pi) / static_cast(kEncoderCPR); + (kWheelDiameterInches * wpi::numbers::pi) / + static_cast(kEncoderCPR); } // namespace DriveConstants namespace ArmConstants { @@ -54,7 +55,8 @@ constexpr auto kMaxAcceleration = 10_rad / (1_s * 1_s); constexpr int kEncoderPorts[]{4, 5}; constexpr int kEncoderPPR = 256; -constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR; +constexpr auto kEncoderDistancePerPulse = + 2.0_rad * wpi::numbers::pi / kEncoderPPR; // The offset of the arm from the horizontal in its neutral position, // measured from the horizontal diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp index b777af3e2e..d85283c9b2 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include /** * This is a sample program to demonstrate how to use a state-space controller @@ -35,7 +35,7 @@ class Robot : public frc::TimedRobot { // distance per pulse = (angle per revolution) / (pulses per revolution) // = (2 * PI rads) / (4096 pulses) static constexpr double kArmEncoderDistPerPulse = - 2.0 * wpi::math::pi / 4096.0; + 2.0 * wpi::numbers::pi / 4096.0; // The arm gearbox represents a gerbox containing two Vex 775pro motors. frc::DCMotor m_armGearbox = frc::DCMotor::Vex775Pro(2); diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h index d7e9e078b6..83832a0ef6 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h @@ -16,7 +16,7 @@ #include #include #include -#include +#include /** * Represents a differential drive style drivetrain. @@ -28,9 +28,9 @@ class Drivetrain { // Set the distance per pulse for the drive encoders. We can simply use the // distance traveled for one rotation of the wheel divided by the encoder // resolution. - m_leftEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + m_leftEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius / kEncoderResolution); - m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + m_rightEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius / kEncoderResolution); m_leftEncoder.Reset(); @@ -40,7 +40,7 @@ class Drivetrain { static constexpr units::meters_per_second_t kMaxSpeed = 3.0_mps; // 3 meters per second static constexpr units::radians_per_second_t kMaxAngularSpeed{ - wpi::math::pi}; // 1/2 rotation per second + wpi::numbers::pi}; // 1/2 rotation per second void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); void Drive(units::meters_per_second_t xSpeed, diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h index e9b657be8a..68fa319a94 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h @@ -16,7 +16,7 @@ #include #include #include -#include +#include /** * Represents a differential drive style drivetrain. @@ -29,9 +29,9 @@ class Drivetrain { // distance traveled for one rotation of the wheel divided by the encoder // resolution. m_leftEncoder.SetDistancePerPulse( - 2 * wpi::math::pi * kWheelRadius.to() / kEncoderResolution); + 2 * wpi::numbers::pi * kWheelRadius.to() / kEncoderResolution); m_rightEncoder.SetDistancePerPulse( - 2 * wpi::math::pi * kWheelRadius.to() / kEncoderResolution); + 2 * wpi::numbers::pi * kWheelRadius.to() / kEncoderResolution); m_leftEncoder.Reset(); m_rightEncoder.Reset(); @@ -40,7 +40,7 @@ class Drivetrain { static constexpr units::meters_per_second_t kMaxSpeed = 3.0_mps; // 3 meters per second static constexpr units::radians_per_second_t kMaxAngularSpeed{ - wpi::math::pi}; // 1/2 rotation per second + wpi::numbers::pi}; // 1/2 rotation per second void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); void Drive(units::meters_per_second_t xSpeed, diff --git a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h index 4e467c9eb9..9d950aa450 100644 --- a/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include /** * The Constants header provides a convenient place for teams to hold robot-wide diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp index 14f34d729f..ebafa75296 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp @@ -12,14 +12,14 @@ #include #include #include -#include +#include class Robot : public frc::TimedRobot { public: static constexpr units::second_t kDt = 20_ms; Robot() { - m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5); + m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::numbers::pi * 1.5); } void TeleopPeriodic() override { diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp index 0aa766fb15..fc6e6d1dcb 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp @@ -17,7 +17,7 @@ #include #include #include -#include +#include /** * This is a sample program to demonstrate how to use a state-space controller @@ -40,7 +40,7 @@ class Robot : public frc::TimedRobot { // distance per pulse = (distance per revolution) / (pulses per revolution) // = (Pi * D) / ppr static constexpr double kArmEncoderDistPerPulse = - 2.0 * wpi::math::pi * kElevatorDrumRadius.to() / 4096.0; + 2.0 * wpi::numbers::pi * kElevatorDrumRadius.to() / 4096.0; // This gearbox represents a gearbox containing 4 Vex 775pro motors. frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4); diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp index cf5fe9906a..d63d84e48d 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include "ExampleSmartMotorController.h" diff --git a/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp index 8a74d53556..7db2c8309a 100644 --- a/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp @@ -5,7 +5,7 @@ #include #include #include -#include +#include /** * Sample program displaying the value of a quadrature encoder on the @@ -40,7 +40,7 @@ class Robot : public frc::TimedRobot { * inch diameter (1.5inch radius) wheel, and that we want to measure * distance in inches. */ - m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5); + m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::numbers::pi * 1.5); /* Defines the lowest rate at which the encoder will not be considered * stopped, for the purposes of the GetStopped() method. Units are in diff --git a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h index 5023c4138c..9c8105b6be 100644 --- a/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h @@ -7,7 +7,7 @@ #include #include #include -#include +#include /** * The Constants header provides a convenient place for teams to hold robot-wide @@ -33,7 +33,8 @@ constexpr int kEncoderCPR = 1024; constexpr double kWheelDiameterInches = 6; constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterInches * wpi::math::pi) / static_cast(kEncoderCPR); + (kWheelDiameterInches * wpi::numbers::pi) / + static_cast(kEncoderCPR); } // namespace DriveConstants namespace ShooterConstants { diff --git a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp index 00becede5f..3ca4cefce1 100644 --- a/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp @@ -7,7 +7,7 @@ #include #include #include -#include +#include DriveTrain::DriveTrain() { // Encoders may measure differently in the real world and in @@ -21,9 +21,9 @@ DriveTrain::DriveTrain() { #else // Circumference = diameter * pi. 360 tick simulated encoders. m_leftEncoder.SetDistancePerPulse(units::foot_t{4_in}.to() * - wpi::math::pi / 360.0); + wpi::numbers::pi / 360.0); m_rightEncoder.SetDistancePerPulse(units::foot_t{4_in}.to() * - wpi::math::pi / 360.0); + wpi::numbers::pi / 360.0); #endif SetName("DriveTrain"); // Let's show everything on the LiveWindow diff --git a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h index b77a926f5b..a7f41d4cc4 100644 --- a/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h @@ -6,7 +6,7 @@ #include #include -#include +#include /** * The Constants header provides a convenient place for teams to hold robot-wide @@ -32,7 +32,8 @@ constexpr int kEncoderCPR = 1024; constexpr double kWheelDiameterInches = 6; constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterInches * wpi::math::pi) / static_cast(kEncoderCPR); + (kWheelDiameterInches * wpi::numbers::pi) / + static_cast(kEncoderCPR); constexpr bool kGyroReversed = true; diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h index 34c47018c3..9299c5129f 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h @@ -4,7 +4,7 @@ #pragma once -#include +#include /** * The Constants header provides a convenient place for teams to hold robot-wide @@ -30,7 +30,8 @@ constexpr int kEncoderCPR = 1024; constexpr double kWheelDiameterInches = 6; constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterInches * wpi::math::pi) / static_cast(kEncoderCPR); + (kWheelDiameterInches * wpi::numbers::pi) / + static_cast(kEncoderCPR); } // namespace DriveConstants namespace HatchConstants { diff --git a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h index 34c47018c3..9299c5129f 100644 --- a/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h @@ -4,7 +4,7 @@ #pragma once -#include +#include /** * The Constants header provides a convenient place for teams to hold robot-wide @@ -30,7 +30,8 @@ constexpr int kEncoderCPR = 1024; constexpr double kWheelDiameterInches = 6; constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterInches * wpi::math::pi) / static_cast(kEncoderCPR); + (kWheelDiameterInches * wpi::numbers::pi) / + static_cast(kEncoderCPR); } // namespace DriveConstants namespace HatchConstants { diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h index 1b097fe489..0aa30868ba 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h @@ -13,7 +13,7 @@ #include #include #include -#include +#include /** * Represents a mecanum drive style drivetrain. @@ -32,7 +32,7 @@ class Drivetrain { static constexpr units::meters_per_second_t kMaxSpeed = 3.0_mps; // 3 meters per second static constexpr units::radians_per_second_t kMaxAngularSpeed{ - wpi::math::pi}; // 1/2 rotation per second + wpi::numbers::pi}; // 1/2 rotation per second private: frc::PWMSparkMax m_frontLeftMotor{1}; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h index 0b486e0711..c2a3047427 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h @@ -12,7 +12,7 @@ #include #include #include -#include +#include #pragma once @@ -51,7 +51,8 @@ constexpr int kEncoderCPR = 1024; constexpr double kWheelDiameterMeters = .15; constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterMeters * wpi::math::pi) / static_cast(kEncoderCPR); + (kWheelDiameterMeters * wpi::numbers::pi) / + static_cast(kEncoderCPR); // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! // These characterization values MUST be determined either experimentally or diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h index ba796d5a0d..d084fa579f 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h @@ -14,7 +14,7 @@ #include #include #include -#include +#include /** * Represents a mecanum drive style drivetrain. @@ -32,7 +32,7 @@ class Drivetrain { static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second static constexpr units::radians_per_second_t kMaxAngularSpeed{ - wpi::math::pi}; // 1/2 rotation per second + wpi::numbers::pi}; // 1/2 rotation per second private: frc::PWMSparkMax m_frontLeftMotor{1}; diff --git a/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp index 5b76a62f80..7d0e9baf96 100644 --- a/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp @@ -7,7 +7,7 @@ #include #include #include -#include +#include /** * This sample program shows how to control a motor using a joystick. In the @@ -35,7 +35,7 @@ class Robot : public frc::TimedRobot { void RobotInit() override { // Use SetDistancePerPulse to set the multiplier for GetDistance // This is set up assuming a 6 inch wheel with a 360 CPR encoder. - m_encoder.SetDistancePerPulse((wpi::math::pi * 6) / 360.0); + m_encoder.SetDistancePerPulse((wpi::numbers::pi * 6) / 360.0); } private: diff --git a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/DriveTrain.cpp b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/DriveTrain.cpp index e9c878a3e6..68447a902a 100644 --- a/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/DriveTrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/DriveTrain.cpp @@ -6,7 +6,7 @@ #include #include -#include +#include #include "commands/DriveWithJoystick.h" @@ -31,9 +31,9 @@ DriveTrain::DriveTrain() : frc::Subsystem("DriveTrain") { #else // Circumference = diameter * pi. 360 tick simulated encoders. m_rightEncoder.SetDistancePerPulse(units::foot_t{4_in}.to() * - wpi::math::pi / 360.0); + wpi::numbers::pi / 360.0); m_leftEncoder.SetDistancePerPulse(units::foot_t{4_in}.to() * - wpi::math::pi / 360.0); + wpi::numbers::pi / 360.0); #endif AddChild("Right Encoder", m_rightEncoder); diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h index a67ae98874..103d8f3137 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include #pragma once @@ -40,7 +40,8 @@ constexpr int kEncoderCPR = 1024; constexpr double kWheelDiameterInches = 6; constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterInches * wpi::math::pi) / static_cast(kEncoderCPR); + (kWheelDiameterInches * wpi::numbers::pi) / + static_cast(kEncoderCPR); // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! // These characterization values MUST be determined either experimentally or diff --git a/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h index 3cdbd1e4d2..b85cc10117 100644 --- a/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h @@ -14,7 +14,7 @@ #include #include #include -#include +#include /** * Represents a differential drive style drivetrain. @@ -26,9 +26,9 @@ class Drivetrain { // Set the distance per pulse for the drive encoders. We can simply use the // distance traveled for one rotation of the wheel divided by the encoder // resolution. - m_leftEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + m_leftEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius / kEncoderResolution); - m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + m_rightEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius / kEncoderResolution); m_leftEncoder.Reset(); @@ -38,7 +38,7 @@ class Drivetrain { static constexpr units::meters_per_second_t kMaxSpeed = 3.0_mps; // 3 meters per second static constexpr units::radians_per_second_t kMaxAngularSpeed{ - wpi::math::pi}; // 1/2 rotation per second + wpi::numbers::pi}; // 1/2 rotation per second void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); void Drive(units::meters_per_second_t xSpeed, diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnDegrees.cpp b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnDegrees.cpp index c59f358a19..2d6c148df1 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnDegrees.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnDegrees.cpp @@ -5,7 +5,7 @@ #include "commands/TurnDegrees.h" #include -#include +#include void TurnDegrees::Initialize() { // Set motors to stop, read encoder values for starting point @@ -26,7 +26,7 @@ bool TurnDegrees::IsFinished() { // found here https://www.pololu.com/category/203/romi-chassis-kits, has a // wheel placement diameter (149 mm) - width of the wheel (8 mm) = 141 mm // or 5.551 inches. We then take into consideration the width of the tires. - static auto inchPerDegree = (5.551_in * wpi::math::pi) / 360_deg; + static auto inchPerDegree = (5.551_in * wpi::numbers::pi) / 360_deg; // Compare distance traveled from start to distance based on degree turn. return GetAverageTurningDistance() >= inchPerDegree * m_angle; diff --git a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp index 4f43eee18d..ba5b109a63 100644 --- a/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp @@ -4,7 +4,7 @@ #include "subsystems/Drivetrain.h" -#include +#include #include "Constants.h" @@ -16,9 +16,9 @@ using namespace DriveConstants; // to use DIO pins 4/5 and 6/7 for the left and right Drivetrain::Drivetrain() { m_leftEncoder.SetDistancePerPulse( - wpi::math::pi * kWheelDiameter.to() / kCountsPerRevolution); + wpi::numbers::pi * kWheelDiameter.to() / kCountsPerRevolution); m_rightEncoder.SetDistancePerPulse( - wpi::math::pi * kWheelDiameter.to() / kCountsPerRevolution); + wpi::numbers::pi * kWheelDiameter.to() / kCountsPerRevolution); ResetEncoders(); } diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h index f60deefc3e..213b5a51c6 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h @@ -22,7 +22,7 @@ #include #include #include -#include +#include /** * Represents a differential drive style drivetrain. @@ -34,9 +34,9 @@ class Drivetrain { // Set the distance per pulse for the drive encoders. We can simply use the // distance traveled for one rotation of the wheel divided by the encoder // resolution. - m_leftEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + m_leftEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius / kEncoderResolution); - m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + m_rightEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius / kEncoderResolution); m_leftEncoder.Reset(); @@ -50,7 +50,7 @@ class Drivetrain { static constexpr units::meters_per_second_t kMaxSpeed = 3.0_mps; // 3 meters per second static constexpr units::radians_per_second_t kMaxAngularSpeed{ - wpi::math::pi}; // 1/2 rotation per second + wpi::numbers::pi}; // 1/2 rotation per second void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds); void Drive(units::meters_per_second_t xSpeed, diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp index 5841f5fb15..39b9197956 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include /** * This is a sample program to demonstrate how to use a state-space controller @@ -66,7 +66,8 @@ class Robot : public frc::TimedRobot { // qelms. Velocity error tolerance, in radians and radians per second. // Decrease this to more heavily penalize state excursion, or make the // controller behave more aggressively. - {1.0 * 2.0 * wpi::math::pi / 360.0, 10.0 * 2.0 * wpi::math::pi / 360.0}, + {1.0 * 2.0 * wpi::numbers::pi / 360.0, + 10.0 * 2.0 * wpi::numbers::pi / 360.0}, // relms. Control effort (voltage) tolerance. Decrease this to more // heavily penalize control effort, or make the controller less // aggressive. 12 is a good starting point because that is the @@ -95,7 +96,7 @@ class Robot : public frc::TimedRobot { public: void RobotInit() override { // We go 2 pi radians per 4096 clicks. - m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0); + m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi / 4096.0); } void TeleopInit() override { diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h index bc350276d7..8a47ff8bb0 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h @@ -12,7 +12,7 @@ #include #include #include -#include +#include #pragma once @@ -43,7 +43,7 @@ constexpr int kEncoderCPR = 1024; constexpr auto kWheelDiameter = 6_in; constexpr double kEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameter.to() * wpi::math::pi) / + (kWheelDiameter.to() * wpi::numbers::pi) / static_cast(kEncoderCPR); // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT! diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp index a0d9eaf717..84efd5c1b4 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include /** * This is a sample program to demonstrate how to use a state-space controller @@ -92,7 +92,7 @@ class Robot : public frc::TimedRobot { public: void RobotInit() override { // Circumference = pi * d, so distance per click = pi * d / counts - m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi * + m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi * kDrumRadius.to() / 4096.0); } diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp index b52fc7ca7d..7d542be5c1 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include /** * This is a sample program to demonstrate how to use a state-space controller @@ -83,7 +83,7 @@ class Robot : public frc::TimedRobot { public: void RobotInit() override { // We go 2 pi radians per 4096 clicks. - m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0); + m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi / 4096.0); } void TeleopInit() override { diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp index 26cebd646b..41068ecd1c 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp @@ -16,7 +16,7 @@ #include #include #include -#include +#include /** * This is a sample program to demonstrate how to use a state-space controller @@ -84,7 +84,7 @@ class Robot : public frc::TimedRobot { public: void RobotInit() override { // We go 2 pi radians per 4096 clicks. - m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0); + m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi / 4096.0); } void TeleopInit() override { diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp index fb07c15050..33397ebd77 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp @@ -5,7 +5,7 @@ #include "SwerveModule.h" #include -#include +#include SwerveModule::SwerveModule(const int driveMotorChannel, const int turningMotorChannel, @@ -20,18 +20,19 @@ SwerveModule::SwerveModule(const int driveMotorChannel, // Set the distance per pulse for the drive encoder. We can simply use the // distance traveled for one rotation of the wheel divided by the encoder // resolution. - m_driveEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius / + m_driveEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius / kEncoderResolution); // Set the distance (in this case, angle) per pulse for the turning encoder. - // This is the the angle through an entire rotation (2 * wpi::math::pi) + // This is the the angle through an entire rotation (2 * wpi::numbers::pi) // divided by the encoder resolution. - m_turningEncoder.SetDistancePerPulse(2 * wpi::math::pi / kEncoderResolution); + m_turningEncoder.SetDistancePerPulse(2 * wpi::numbers::pi / + kEncoderResolution); // Limit the PID Controller's input range between -pi and pi and set the input // to be continuous. - m_turningPIDController.EnableContinuousInput(-units::radian_t(wpi::math::pi), - units::radian_t(wpi::math::pi)); + m_turningPIDController.EnableContinuousInput( + -units::radian_t(wpi::numbers::pi), units::radian_t(wpi::numbers::pi)); } frc::SwerveModuleState SwerveModule::GetState() const { diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h index 496ba7f39c..6ff5b48b38 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include "SwerveModule.h" @@ -27,7 +27,7 @@ class Drivetrain { static constexpr units::meters_per_second_t kMaxSpeed = 3.0_mps; // 3 meters per second static constexpr units::radians_per_second_t kMaxAngularSpeed{ - wpi::math::pi}; // 1/2 rotation per second + wpi::numbers::pi}; // 1/2 rotation per second private: frc::Translation2d m_frontLeftLocation{+0.381_m, +0.381_m}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h index db02c729d5..00a938b83f 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h @@ -14,7 +14,7 @@ #include #include #include -#include +#include class SwerveModule { public: @@ -29,9 +29,9 @@ class SwerveModule { static constexpr int kEncoderResolution = 4096; static constexpr auto kModuleMaxAngularVelocity = - wpi::math::pi * 1_rad_per_s; // radians per second + wpi::numbers::pi * 1_rad_per_s; // radians per second static constexpr auto kModuleMaxAngularAcceleration = - wpi::math::pi * 2_rad_per_s / 1_s; // radians per second^2 + wpi::numbers::pi * 2_rad_per_s / 1_s; // radians per second^2 frc::PWMSparkMax m_driveMotor; frc::PWMSparkMax m_turningMotor; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp index 552a5b0c93..1778aefd36 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp @@ -67,8 +67,8 @@ frc2::Command* RobotContainer::GetAutonomousCommand() { AutoConstants::kPThetaController, 0, 0, AutoConstants::kThetaControllerConstraints}; - thetaController.EnableContinuousInput(units::radian_t(-wpi::math::pi), - units::radian_t(wpi::math::pi)); + thetaController.EnableContinuousInput(units::radian_t(-wpi::numbers::pi), + units::radian_t(wpi::numbers::pi)); frc2::SwerveControllerCommand<4> swerveControllerCommand( exampleTrajectory, [this]() { return m_drive.GetPose(); }, diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp index bccafd7b75..f75e475b9f 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp @@ -5,7 +5,7 @@ #include "subsystems/SwerveModule.h" #include -#include +#include #include "Constants.h" @@ -27,15 +27,15 @@ SwerveModule::SwerveModule(int driveMotorChannel, int turningMotorChannel, ModuleConstants::kDriveEncoderDistancePerPulse); // Set the distance (in this case, angle) per pulse for the turning encoder. - // This is the the angle through an entire rotation (2 * wpi::math::pi) + // This is the the angle through an entire rotation (2 * wpi::numbers::pi) // divided by the encoder resolution. m_turningEncoder.SetDistancePerPulse( ModuleConstants::kTurningEncoderDistancePerPulse); // Limit the PID Controller's input range between -pi and pi and set the input // to be continuous. - m_turningPIDController.EnableContinuousInput(units::radian_t(-wpi::math::pi), - units::radian_t(wpi::math::pi)); + m_turningPIDController.EnableContinuousInput( + units::radian_t(-wpi::numbers::pi), units::radian_t(wpi::numbers::pi)); } frc::SwerveModuleState SwerveModule::GetState() { diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h index d41eeda548..96d1c25a6b 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h @@ -12,7 +12,7 @@ #include #include #include -#include +#include #pragma once @@ -77,11 +77,12 @@ constexpr int kEncoderCPR = 1024; constexpr double kWheelDiameterMeters = .15; constexpr double kDriveEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (kWheelDiameterMeters * wpi::math::pi) / static_cast(kEncoderCPR); + (kWheelDiameterMeters * wpi::numbers::pi) / + static_cast(kEncoderCPR); constexpr double kTurningEncoderDistancePerPulse = // Assumes the encoders are directly mounted on the wheel shafts - (wpi::math::pi * 2) / static_cast(kEncoderCPR); + (wpi::numbers::pi * 2) / static_cast(kEncoderCPR); constexpr double kPModuleTurningController = 1; constexpr double kPModuleDriveController = 1; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h index 304d8736c6..4208b3551a 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include "Constants.h" @@ -37,11 +37,11 @@ class SwerveModule { // meters per second squared. static constexpr units::radians_per_second_t kModuleMaxAngularVelocity = - units::radians_per_second_t(wpi::math::pi); // radians per second + units::radians_per_second_t(wpi::numbers::pi); // radians per second static constexpr units::unit_t kModuleMaxAngularAcceleration = units::unit_t( - wpi::math::pi * 2.0); // radians per second squared + wpi::numbers::pi * 2.0); // radians per second squared frc::Spark m_driveMotor; frc::Spark m_turningMotor; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp index e7879ff658..3091da8c9e 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp @@ -5,7 +5,7 @@ #include "SwerveModule.h" #include -#include +#include SwerveModule::SwerveModule(const int driveMotorChannel, const int turningMotorChannel, @@ -21,17 +21,18 @@ SwerveModule::SwerveModule(const int driveMotorChannel, // distance traveled for one rotation of the wheel divided by the encoder // resolution. m_driveEncoder.SetDistancePerPulse( - 2 * wpi::math::pi * kWheelRadius.to() / kEncoderResolution); + 2 * wpi::numbers::pi * kWheelRadius.to() / kEncoderResolution); // Set the distance (in this case, angle) per pulse for the turning encoder. - // This is the the angle through an entire rotation (2 * wpi::math::pi) + // This is the the angle through an entire rotation (2 * wpi::numbers::pi) // divided by the encoder resolution. - m_turningEncoder.SetDistancePerPulse(2 * wpi::math::pi / kEncoderResolution); + m_turningEncoder.SetDistancePerPulse(2 * wpi::numbers::pi / + kEncoderResolution); // Limit the PID Controller's input range between -pi and pi and set the input // to be continuous. - m_turningPIDController.EnableContinuousInput(-units::radian_t(wpi::math::pi), - units::radian_t(wpi::math::pi)); + m_turningPIDController.EnableContinuousInput( + -units::radian_t(wpi::numbers::pi), units::radian_t(wpi::numbers::pi)); } frc::SwerveModuleState SwerveModule::GetState() const { diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h index c75002f130..81f8bb7685 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include "SwerveModule.h" @@ -27,7 +27,7 @@ class Drivetrain { static constexpr auto kMaxSpeed = 3.0_mps; // 3 meters per second static constexpr units::radians_per_second_t kMaxAngularSpeed{ - wpi::math::pi}; // 1/2 rotation per second + wpi::numbers::pi}; // 1/2 rotation per second private: frc::Translation2d m_frontLeftLocation{+0.381_m, +0.381_m}; diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h index 8ce7adea58..7456584730 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h @@ -14,7 +14,7 @@ #include #include #include -#include +#include class SwerveModule { public: @@ -29,9 +29,9 @@ class SwerveModule { static constexpr int kEncoderResolution = 4096; static constexpr auto kModuleMaxAngularVelocity = - wpi::math::pi * 1_rad_per_s; // radians per second + wpi::numbers::pi * 1_rad_per_s; // radians per second static constexpr auto kModuleMaxAngularAcceleration = - wpi::math::pi * 2_rad_per_s / 1_s; // radians per second^2 + wpi::numbers::pi * 2_rad_per_s / 1_s; // radians per second^2 frc::PWMSparkMax m_driveMotor; frc::PWMSparkMax m_turningMotor; diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java index 98c636fd2f..5647cfe4c9 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java @@ -73,8 +73,8 @@ public class SwerveModule { m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); // Set the distance (in this case, angle) per pulse for the turning encoder. - // This is the the angle through an entire rotation (2 * wpi::math::pi) - // divided by the encoder resolution. + // This is the the angle through an entire rotation (2 * pi) divided by the + // encoder resolution. m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution); // Limit the PID Controller's input range between -pi and pi and set the input diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java index 2bc737f42b..fee563f33a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java @@ -63,8 +63,8 @@ public class SwerveModule { m_driveEncoder.setReverseDirection(driveEncoderReversed); // Set the distance (in this case, angle) per pulse for the turning encoder. - // This is the the angle through an entire rotation (2 * wpi::math::pi) - // divided by the encoder resolution. + // This is the the angle through an entire rotation (2 * pi) divided by the + // encoder resolution. m_turningEncoder.setDistancePerPulse(ModuleConstants.kTurningEncoderDistancePerPulse); // Set whether turning encoder should be reversed or not diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java index d914c95f6f..bc5d89e38d 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java @@ -73,8 +73,8 @@ public class SwerveModule { m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution); // Set the distance (in this case, angle) per pulse for the turning encoder. - // This is the the angle through an entire rotation (2 * wpi::math::pi) - // divided by the encoder resolution. + // This is the the angle through an entire rotation (2 * pi) divided by the + // encoder resolution. m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution); // Limit the PID Controller's input range between -pi and pi and set the input diff --git a/wpimath/src/dev/native/cpp/main.cpp b/wpimath/src/dev/native/cpp/main.cpp index e94f09cc01..85398058e9 100644 --- a/wpimath/src/dev/native/cpp/main.cpp +++ b/wpimath/src/dev/native/cpp/main.cpp @@ -4,8 +4,8 @@ #include -#include +#include int main() { - std::cout << wpi::math::pi << std::endl; + std::cout << wpi::numbers::pi << std::endl; } diff --git a/wpimath/src/main/native/include/frc/MathUtil.h b/wpimath/src/main/native/include/frc/MathUtil.h index 274f6d5f07..2cef244e57 100644 --- a/wpimath/src/main/native/include/frc/MathUtil.h +++ b/wpimath/src/main/native/include/frc/MathUtil.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #include "units/angle.h" @@ -38,8 +38,9 @@ constexpr T InputModulus(T input, T minimumInput, T maximumInput) { * @param angle Angle to wrap. */ constexpr units::radian_t AngleModulus(units::radian_t angle) { - return InputModulus(angle, units::radian_t{-wpi::math::pi}, - units::radian_t{wpi::math::pi}); + return InputModulus(angle, + units::radian_t{-wpi::numbers::pi}, + units::radian_t{wpi::numbers::pi}); } } // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h index 9e2ed8a956..9b4a958f27 100644 --- a/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h +++ b/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #include "Eigen/Core" #include "frc/MathUtil.h" @@ -59,7 +59,7 @@ Eigen::Matrix AngleAdd( const Eigen::Matrix& b, int angleStateIdx) { Eigen::Matrix ret = a + b; ret[angleStateIdx] = - InputModulus(ret[angleStateIdx], -wpi::math::pi, wpi::math::pi); + InputModulus(ret[angleStateIdx], -wpi::numbers::pi, wpi::numbers::pi); return ret; } diff --git a/wpimath/src/main/native/include/units/math.h b/wpimath/src/main/native/include/units/math.h index cba14c44f0..995335bbb5 100644 --- a/wpimath/src/main/native/include/units/math.h +++ b/wpimath/src/main/native/include/units/math.h @@ -28,8 +28,6 @@ #include -#include - #include "units/angle.h" #include "units/base.h" #include "units/dimensionless.h" diff --git a/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp b/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp index d7cb6e15dc..8d0e613640 100644 --- a/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp +++ b/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp @@ -8,7 +8,7 @@ #include #include -#include +#include #include "gtest/gtest.h" #include "units/time.h" @@ -36,7 +36,7 @@ std::ostream& operator<<(std::ostream& os, } static double GetData(double t) { - return 100.0 * std::sin(2.0 * wpi::math::pi * t); + return 100.0 * std::sin(2.0 * wpi::numbers::pi * t); } class LinearFilterNoiseTest diff --git a/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp b/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp index c62a1e82f6..b69d1e5bfc 100644 --- a/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp +++ b/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include "gtest/gtest.h" #include "units/time.h" @@ -52,8 +52,8 @@ std::ostream& operator<<(std::ostream& os, } static double GetData(double t) { - return 100.0 * std::sin(2.0 * wpi::math::pi * t) + - 20.0 * std::cos(50.0 * wpi::math::pi * t); + return 100.0 * std::sin(2.0 * wpi::numbers::pi * t) + + 20.0 * std::cos(50.0 * wpi::numbers::pi * t); } static double GetPulseData(double t) { diff --git a/wpimath/src/test/native/cpp/MathUtilTest.cpp b/wpimath/src/test/native/cpp/MathUtilTest.cpp index ee4fc6dc86..8ef4f0b455 100644 --- a/wpimath/src/test/native/cpp/MathUtilTest.cpp +++ b/wpimath/src/test/native/cpp/MathUtilTest.cpp @@ -51,19 +51,20 @@ TEST(MathUtilTest, InputModulus) { TEST(MathUtilTest, AngleModulus) { EXPECT_UNITS_EQ( - frc::AngleModulus(units::radian_t{-2000 * wpi::math::pi / 180}), - units::radian_t{160 * wpi::math::pi / 180}); - EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{358 * wpi::math::pi / 180}), - units::radian_t{-2 * wpi::math::pi / 180}); - EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{2.0 * wpi::math::pi}), + frc::AngleModulus(units::radian_t{-2000 * wpi::numbers::pi / 180}), + units::radian_t{160 * wpi::numbers::pi / 180}); + EXPECT_UNITS_EQ( + frc::AngleModulus(units::radian_t{358 * wpi::numbers::pi / 180}), + units::radian_t{-2 * wpi::numbers::pi / 180}); + EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t{2.0 * wpi::numbers::pi}), 0_rad); - EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(5 * wpi::math::pi)), - units::radian_t(wpi::math::pi)); - EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-5 * wpi::math::pi)), - units::radian_t(wpi::math::pi)); - EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(wpi::math::pi / 2)), - units::radian_t(wpi::math::pi / 2)); - EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-wpi::math::pi / 2)), - units::radian_t(-wpi::math::pi / 2)); + EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(5 * wpi::numbers::pi)), + units::radian_t(wpi::numbers::pi)); + EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-5 * wpi::numbers::pi)), + units::radian_t(wpi::numbers::pi)); + EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(wpi::numbers::pi / 2)), + units::radian_t(wpi::numbers::pi / 2)); + EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-wpi::numbers::pi / 2)), + units::radian_t(-wpi::numbers::pi / 2)); } diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp index bbbc398414..bd1e30b6d4 100644 --- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp @@ -4,15 +4,15 @@ #include -#include +#include #include "Eigen/Core" #include "frc/estimator/AngleStatistics.h" TEST(AngleStatisticsTest, TestMean) { Eigen::Matrix sigmas; - sigmas << 1, 1.2, 0, 359 * wpi::math::pi / 180, 3 * wpi::math::pi / 180, 0, 1, - 2, 0; + sigmas << 1, 1.2, 0, 359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180, + 0, 1, 2, 0; // Weights need to produce the mean of the sigmas Eigen::Vector3d weights{}; weights.fill(1.0 / sigmas.cols()); @@ -22,16 +22,16 @@ TEST(AngleStatisticsTest, TestMean) { } TEST(AngleStatisticsTest, TestResidual) { - Eigen::Vector3d a(1, 1 * wpi::math::pi / 180, 2); - Eigen::Vector3d b(1, 359 * wpi::math::pi / 180, 1); + Eigen::Vector3d a(1, 1 * wpi::numbers::pi / 180, 2); + Eigen::Vector3d b(1, 359 * wpi::numbers::pi / 180, 1); EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox( - Eigen::Vector3d(0, 2 * wpi::math::pi / 180, 1))); + Eigen::Vector3d(0, 2 * wpi::numbers::pi / 180, 1))); } TEST(AngleStatisticsTest, TestAdd) { - Eigen::Vector3d a(1, 1 * wpi::math::pi / 180, 2); - Eigen::Vector3d b(1, 359 * wpi::math::pi / 180, 1); + Eigen::Vector3d a(1, 1 * wpi::numbers::pi / 180, 2); + Eigen::Vector3d b(1, 359 * wpi::numbers::pi / 180, 1); EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d(2, 0, 3))); } diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 0597cd3218..006267d2ea 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -4,7 +4,7 @@ #include -#include +#include #include "frc/geometry/Rotation2d.h" #include "gtest/gtest.h" @@ -14,8 +14,8 @@ using namespace frc; static constexpr double kEpsilon = 1E-9; TEST(Rotation2dTest, RadiansToDegrees) { - const Rotation2d rot1{units::radian_t(wpi::math::pi / 3)}; - const Rotation2d rot2{units::radian_t(wpi::math::pi / 4)}; + const Rotation2d rot1{units::radian_t(wpi::numbers::pi / 3)}; + const Rotation2d rot2{units::radian_t(wpi::numbers::pi / 4)}; EXPECT_NEAR(rot1.Degrees().to(), 60.0, kEpsilon); EXPECT_NEAR(rot2.Degrees().to(), 45.0, kEpsilon); @@ -25,15 +25,15 @@ TEST(Rotation2dTest, DegreesToRadians) { const auto rot1 = Rotation2d(45.0_deg); const auto rot2 = Rotation2d(30.0_deg); - EXPECT_NEAR(rot1.Radians().to(), wpi::math::pi / 4.0, kEpsilon); - EXPECT_NEAR(rot2.Radians().to(), wpi::math::pi / 6.0, kEpsilon); + EXPECT_NEAR(rot1.Radians().to(), wpi::numbers::pi / 4.0, kEpsilon); + EXPECT_NEAR(rot2.Radians().to(), wpi::numbers::pi / 6.0, kEpsilon); } TEST(Rotation2dTest, RotateByFromZero) { const Rotation2d zero; auto sum = zero + Rotation2d(90.0_deg); - EXPECT_NEAR(sum.Radians().to(), wpi::math::pi / 2.0, kEpsilon); + EXPECT_NEAR(sum.Radians().to(), wpi::numbers::pi / 2.0, kEpsilon); EXPECT_NEAR(sum.Degrees().to(), 90.0, kEpsilon); } diff --git a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp index b8760e5fc4..19b56fe183 100644 --- a/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp @@ -4,7 +4,7 @@ #include -#include +#include #include "frc/geometry/Pose2d.h" #include "gtest/gtest.h" @@ -23,8 +23,8 @@ TEST(Twist2dTest, Straight) { } TEST(Twist2dTest, QuarterCircle) { - const Twist2d quarterCircle{5.0_m / 2.0 * wpi::math::pi, 0_m, - units::radian_t(wpi::math::pi / 2.0)}; + const Twist2d quarterCircle{5.0_m / 2.0 * wpi::numbers::pi, 0_m, + units::radian_t(wpi::numbers::pi / 2.0)}; const auto quarterCirclePose = Pose2d().Exp(quarterCircle); EXPECT_NEAR(quarterCirclePose.X().to(), 5.0, kEpsilon); @@ -60,7 +60,7 @@ TEST(Twist2dTest, Pose2dLog) { const auto twist = start.Log(end); - EXPECT_NEAR(twist.dx.to(), 5 / 2.0 * wpi::math::pi, kEpsilon); + EXPECT_NEAR(twist.dx.to(), 5 / 2.0 * wpi::numbers::pi, kEpsilon); EXPECT_NEAR(twist.dy.to(), 0.0, kEpsilon); - EXPECT_NEAR(twist.dtheta.to(), wpi::math::pi / 2.0, kEpsilon); + EXPECT_NEAR(twist.dtheta.to(), wpi::numbers::pi / 2.0, kEpsilon); } diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp index 4647b02be4..dea3b5557e 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include +#include #include "frc/kinematics/ChassisSpeeds.h" #include "frc/kinematics/DifferentialDriveKinematics.h" @@ -55,22 +55,24 @@ TEST(DifferentialDriveKinematics, ForwardKinematicsForStraightLine) { TEST(DifferentialDriveKinematics, InverseKinematicsForRotateInPlace) { const DifferentialDriveKinematics kinematics{0.381_m * 2}; - const ChassisSpeeds chassisSpeeds{0.0_mps, 0.0_mps, - units::radians_per_second_t{wpi::math::pi}}; + const ChassisSpeeds chassisSpeeds{ + 0.0_mps, 0.0_mps, units::radians_per_second_t{wpi::numbers::pi}}; const auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds); - EXPECT_NEAR(wheelSpeeds.left.to(), -0.381 * wpi::math::pi, kEpsilon); - EXPECT_NEAR(wheelSpeeds.right.to(), +0.381 * wpi::math::pi, kEpsilon); + EXPECT_NEAR(wheelSpeeds.left.to(), -0.381 * wpi::numbers::pi, + kEpsilon); + EXPECT_NEAR(wheelSpeeds.right.to(), +0.381 * wpi::numbers::pi, + kEpsilon); } TEST(DifferentialDriveKinematics, ForwardKinematicsForRotateInPlace) { const DifferentialDriveKinematics kinematics{0.381_m * 2}; const DifferentialDriveWheelSpeeds wheelSpeeds{ - units::meters_per_second_t(+0.381 * wpi::math::pi), - units::meters_per_second_t(-0.381 * wpi::math::pi)}; + units::meters_per_second_t(+0.381 * wpi::numbers::pi), + units::meters_per_second_t(-0.381 * wpi::numbers::pi)}; const auto chassisSpeeds = kinematics.ToChassisSpeeds(wheelSpeeds); EXPECT_NEAR(chassisSpeeds.vx.to(), 0, kEpsilon); EXPECT_NEAR(chassisSpeeds.vy.to(), 0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.omega.to(), -wpi::math::pi, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.to(), -wpi::numbers::pi, kEpsilon); } diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp index f7bb4df064..31b1113460 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include +#include #include "frc/kinematics/DifferentialDriveKinematics.h" #include "frc/kinematics/DifferentialDriveOdometry.h" @@ -16,7 +16,7 @@ TEST(DifferentialDriveOdometry, EncoderDistances) { DifferentialDriveOdometry odometry{Rotation2d(45_deg)}; const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m, - units::meter_t(5 * wpi::math::pi)); + units::meter_t(5 * wpi::numbers::pi)); EXPECT_NEAR(pose.X().to(), 5.0, kEpsilon); EXPECT_NEAR(pose.Y().to(), 5.0, kEpsilon); diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp index f9eaa8bfef..9834aa0eab 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include +#include #include "frc/geometry/Translation2d.h" #include "frc/kinematics/MecanumDriveKinematics.h" @@ -61,7 +61,7 @@ TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) { TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) { ChassisSpeeds speeds{0_mps, 0_mps, - units::radians_per_second_t(2 * wpi::math::pi)}; + units::radians_per_second_t(2 * wpi::numbers::pi)}; auto moduleStates = kinematics.ToWheelSpeeds(speeds); EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.to(), 0.1); @@ -77,7 +77,7 @@ TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) { EXPECT_NEAR(0.0, chassisSpeeds.vx.to(), 0.1); EXPECT_NEAR(0.0, chassisSpeeds.vy.to(), 0.1); - EXPECT_NEAR(2 * wpi::math::pi, chassisSpeeds.omega.to(), 0.1); + EXPECT_NEAR(2 * wpi::numbers::pi, chassisSpeeds.omega.to(), 0.1); } TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationInverseKinematics) { diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index 18923a96ba..6e4beca542 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -#include +#include #include "frc/geometry/Translation2d.h" #include "frc/kinematics/SwerveDriveKinematics.h" @@ -75,7 +75,7 @@ TEST_F(SwerveDriveKinematicsTest, StraightStrafeForwardKinematics) { TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) { ChassisSpeeds speeds{0_mps, 0_mps, - units::radians_per_second_t(2 * wpi::math::pi)}; + units::radians_per_second_t(2 * wpi::numbers::pi)}; auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds); EXPECT_NEAR(fl.speed.to(), 106.63, kEpsilon); @@ -99,12 +99,12 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) { EXPECT_NEAR(chassisSpeeds.vx.to(), 0.0, kEpsilon); EXPECT_NEAR(chassisSpeeds.vy.to(), 0.0, kEpsilon); - EXPECT_NEAR(chassisSpeeds.omega.to(), 2 * wpi::math::pi, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.to(), 2 * wpi::numbers::pi, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) { ChassisSpeeds speeds{0_mps, 0_mps, - units::radians_per_second_t(2 * wpi::math::pi)}; + units::radians_per_second_t(2 * wpi::numbers::pi)}; auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(speeds, m_fl); EXPECT_NEAR(fl.speed.to(), 0.0, kEpsilon); @@ -128,7 +128,7 @@ TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) { EXPECT_NEAR(chassisSpeeds.vx.to(), 75.398, kEpsilon); EXPECT_NEAR(chassisSpeeds.vy.to(), -75.398, kEpsilon); - EXPECT_NEAR(chassisSpeeds.omega.to(), 2 * wpi::math::pi, kEpsilon); + EXPECT_NEAR(chassisSpeeds.omega.to(), 2 * wpi::numbers::pi, kEpsilon); } TEST_F(SwerveDriveKinematicsTest, diff --git a/wpiutil/.styleguide b/wpiutil/.styleguide index 0b07517c75..f9dddff184 100644 --- a/wpiutil/.styleguide +++ b/wpiutil/.styleguide @@ -2,6 +2,8 @@ cppHeaderFileInclude { \.h$ \.inc$ \.inl$ + math$ + numbers$ } cppSrcFileInclude { diff --git a/wpiutil/src/main/native/include/wpi/math b/wpiutil/src/main/native/include/wpi/math index d9c18c7f68..25f2a64aac 100644 --- a/wpiutil/src/main/native/include/wpi/math +++ b/wpiutil/src/main/native/include/wpi/math @@ -1,67 +1,20 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 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. */ -/*----------------------------------------------------------------------------*/ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. #pragma once -#include +#include "wpi/numbers" + +// clang-format off +#ifdef _MSC_VER +#pragma message("warning: Use and wpi::numbers instead to reflect C++20 and std::numbers") +#else +#warning "Use and wpi::numbers instead to reflect C++20 and std::numbers" +#endif + +// clang-format on namespace wpi::math { - -template >> -inline constexpr T e_v = 2.718281828459045235360287471352662498L; - -template >> -inline constexpr T log2e_v = 1.442695040888963407359924681001892137L; - -template >> -inline constexpr T log10e_v = 0.434294481903251827651128918916605082L; - -template >> -inline constexpr T pi_v = 3.141592653589793238462643383279502884L; - -template >> -inline constexpr T inv_pi_v = 0.318309886183790671537767526745028724L; - -template >> -inline constexpr T inv_sqrtpi_v = 0.564189583547756286948079451560772586L; - -template >> -inline constexpr T ln2_v = 0.693147180559945309417232121458176568L; - -template >> -inline constexpr T ln10_v = 2.302585092994045684017991454684364208L; - -template >> -inline constexpr T sqrt2_v = 1.414213562373095048801688724209698078L; - -template >> -inline constexpr T sqrt3_v = 1.732050807568877293527446341505872366L; - -template >> -inline constexpr T inv_sqrt3_v = 0.577350269189625764509148780501957456L; - -template >> -inline constexpr T egamma_v = 0.577215664901532860606512090082402431L; - -template >> -inline constexpr T phi_v = 1.618033988749894848204586834365638117L; - -inline constexpr double e = e_v; -inline constexpr double log2e = log2e_v; -inline constexpr double log10e = log10e_v; -inline constexpr double pi = pi_v; -inline constexpr double inv_pi = inv_pi_v; -inline constexpr double inv_sqrtpi = inv_sqrtpi_v; -inline constexpr double ln2 = ln2_v; -inline constexpr double ln10 = ln10_v; -inline constexpr double sqrt2 = sqrt2_v; -inline constexpr double sqrt3 = sqrt3_v; -inline constexpr double inv_sqrt3 = inv_sqrt3_v; -inline constexpr double egamma = egamma_v; -inline constexpr double phi = phi_v; - +using namespace wpi::numbers; } // namespace wpi::math diff --git a/wpiutil/src/main/native/include/wpi/numbers b/wpiutil/src/main/native/include/wpi/numbers new file mode 100644 index 0000000000..d45aee0420 --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/numbers @@ -0,0 +1,64 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +namespace wpi::numbers { + +template >> +inline constexpr T e_v = 2.718281828459045235360287471352662498L; + +template >> +inline constexpr T log2e_v = 1.442695040888963407359924681001892137L; + +template >> +inline constexpr T log10e_v = 0.434294481903251827651128918916605082L; + +template >> +inline constexpr T pi_v = 3.141592653589793238462643383279502884L; + +template >> +inline constexpr T inv_pi_v = 0.318309886183790671537767526745028724L; + +template >> +inline constexpr T inv_sqrtpi_v = 0.564189583547756286948079451560772586L; + +template >> +inline constexpr T ln2_v = 0.693147180559945309417232121458176568L; + +template >> +inline constexpr T ln10_v = 2.302585092994045684017991454684364208L; + +template >> +inline constexpr T sqrt2_v = 1.414213562373095048801688724209698078L; + +template >> +inline constexpr T sqrt3_v = 1.732050807568877293527446341505872366L; + +template >> +inline constexpr T inv_sqrt3_v = 0.577350269189625764509148780501957456L; + +template >> +inline constexpr T egamma_v = 0.577215664901532860606512090082402431L; + +template >> +inline constexpr T phi_v = 1.618033988749894848204586834365638117L; + +inline constexpr double e = e_v; +inline constexpr double log2e = log2e_v; +inline constexpr double log10e = log10e_v; +inline constexpr double pi = pi_v; +inline constexpr double inv_pi = inv_pi_v; +inline constexpr double inv_sqrtpi = inv_sqrtpi_v; +inline constexpr double ln2 = ln2_v; +inline constexpr double ln10 = ln10_v; +inline constexpr double sqrt2 = sqrt2_v; +inline constexpr double sqrt3 = sqrt3_v; +inline constexpr double inv_sqrt3 = inv_sqrt3_v; +inline constexpr double egamma = egamma_v; +inline constexpr double phi = phi_v; + +} // namespace wpi::numbers