From 00fa8361ddeb9e78aa9997cd5006556953b797bb Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Mon, 12 Jan 2026 19:09:35 -0800 Subject: [PATCH] [wpimath] Reorganize LinearSystem factories (#8468) --- docs/build.gradle | 2 - .../native/cpp/analysis/FeedbackAnalysis.cpp | 18 +- .../simulation/DifferentialDrivetrainSim.cpp | 4 +- .../native/cpp/simulation/ElevatorSim.cpp | 10 +- .../cpp/simulation/SingleJointedArmSim.cpp | 11 +- .../include/wpi/simulation/DCMotorSim.hpp | 13 +- .../simulation/DifferentialDrivetrainSim.hpp | 65 +- .../include/wpi/simulation/ElevatorSim.hpp | 58 +- .../include/wpi/simulation/FlywheelSim.hpp | 12 +- .../wpi/simulation/SingleJointedArmSim.hpp | 43 +- .../native/cpp/simulation/DCMotorSimTest.cpp | 6 +- .../DifferentialDrivetrainSimTest.cpp | 10 +- .../native/cpp/simulation/ElevatorSimTest.cpp | 6 +- .../cpp/simulation/StateSpaceSimTest.cpp | 6 +- .../cpp/Drivetrain.cpp | 4 + .../include/Drivetrain.hpp | 9 +- .../FlywheelBangBangController/cpp/Robot.cpp | 4 +- .../include/Drivetrain.hpp | 1 - .../include/Drivetrain.hpp | 5 +- .../cpp/examples/StateSpaceArm/cpp/Robot.cpp | 7 +- .../examples/StateSpaceElevator/cpp/Robot.cpp | 7 +- .../examples/StateSpaceFlywheel/cpp/Robot.cpp | 12 +- .../StateSpaceFlywheelSysId/cpp/Robot.cpp | 8 +- .../include/Drivetrain.hpp | 1 - .../cpp/PotentiometerPIDTest.cpp | 2 +- .../org/wpilib/simulation/DCMotorSim.java | 9 +- .../simulation/DifferentialDrivetrainSim.java | 11 +- .../org/wpilib/simulation/ElevatorSim.java | 12 +- .../org/wpilib/simulation/FlywheelSim.java | 9 +- .../simulation/SingleJointedArmSim.java | 8 +- .../org/wpilib/simulation/DCMotorSimTest.java | 10 +- .../DifferentialDrivetrainSimTest.java | 10 +- .../wpilib/simulation/ElevatorSimTest.java | 6 +- .../simulation/SingleJointedArmSimTest.java | 2 +- .../armsimulation/subsystems/Arm.java | 2 +- .../Drivetrain.java | 6 +- .../subsystems/Elevator.java | 2 +- .../subsystems/Elevator.java | 2 +- .../flywheelbangbangcontroller/Robot.java | 6 +- .../Drivetrain.java | 6 +- .../wpilib/examples/statespacearm/Robot.java | 6 +- .../examples/statespaceelevator/Robot.java | 6 +- .../examples/statespaceflywheel/Robot.java | 6 +- .../statespaceflywheelsysid/Robot.java | 4 +- .../PotentiometerPIDTest.java | 2 +- wpimath/robotpy_pybind_build_info.bzl | 28 +- .../java/org/wpilib/math/proto/Plant.java | 621 ------------------ .../java/org/wpilib/math/proto/System.java | 620 ++++++++++++++++- .../native/cpp/wpimath/protobuf/plant.npb.cpp | 92 --- .../native/cpp/wpimath/protobuf/plant.npb.h | 57 -- .../cpp/wpimath/protobuf/system.npb.cpp | 204 ++++-- .../native/cpp/wpimath/protobuf/system.npb.h | 30 + .../DifferentialDriveAccelerationLimiter.java | 2 +- .../DifferentialDriveFeedforward.java | 6 +- .../math/system/{plant => }/DCMotor.java | 6 +- .../java/org/wpilib/math/system/Models.java | 370 +++++++++++ .../math/system/plant/LinearSystemId.java | 390 ----------- .../{plant => }/proto/DCMotorProto.java | 6 +- .../{plant => }/struct/DCMotorStruct.java | 4 +- .../system/{plant => }/proto/DCMotorProto.cpp | 4 +- .../{plant => }/struct/DCMotorStruct.cpp | 2 +- .../DifferentialDriveAccelerationLimiter.hpp | 3 +- .../DifferentialDriveFeedforward.hpp | 6 +- .../math/controller/ElevatorFeedforward.hpp | 42 -- .../wpi/math/system/{plant => }/DCMotor.hpp | 4 +- .../{plant/LinearSystemId.hpp => Models.hpp} | 499 ++++++-------- .../system/{plant => }/proto/DCMotorProto.hpp | 4 +- .../{plant => }/struct/DCMotorStruct.hpp | 2 +- wpimath/src/main/proto/plant.proto | 13 - wpimath/src/main/proto/system.proto | 8 + wpimath/src/main/python/pyproject.toml | 6 +- .../python/semiwrap/ElevatorFeedforward.yml | 4 - .../main/python/semiwrap/LinearSystemId.yml | 59 -- wpimath/src/main/python/semiwrap/Models.yml | 14 + wpimath/src/main/python/wpimath/__init__.py | 4 +- ...ferentialDriveAccelerationLimiterTest.java | 10 +- .../DifferentialDriveFeedforwardTest.java | 7 +- .../controller/ImplicitModelFollowerTest.java | 8 +- .../LTVDifferentialDriveControllerTest.java | 4 +- .../LinearQuadraticRegulatorTest.java | 12 +- .../math/controller/LinearSystemLoopTest.java | 8 +- .../estimator/ExtendedKalmanFilterTest.java | 2 +- .../math/estimator/KalmanFilterTest.java | 6 +- .../wpilib/math/estimator/MerweUKFTest.java | 6 +- .../org/wpilib/math/estimator/S3UKFTest.java | 6 +- ...inearSystemIDTest.java => ModelsTest.java} | 79 +-- .../{plant => }/proto/DCMotorProtoTest.java | 6 +- .../{plant => }/struct/DCMotorStructTest.java | 4 +- .../src/test/native/cpp/StateSpaceTest.cpp | 7 +- ...fferentialDriveAccelerationLimiterTest.cpp | 18 +- .../DifferentialDriveFeedforwardTest.cpp | 11 +- .../controller/ElevatorFeedforwardTest.cpp | 4 +- .../controller/ImplicitModelFollowerTest.cpp | 14 +- .../LTVDifferentialDriveControllerTest.cpp | 4 +- .../LinearPlantInversionFeedforwardTest.cpp | 2 - .../LinearQuadraticRegulatorTest.cpp | 17 +- .../controller/SimpleMotorFeedforwardTest.cpp | 2 - .../DifferentialDrivePoseEstimator3dTest.cpp | 1 - .../DifferentialDrivePoseEstimatorTest.cpp | 1 - .../estimator/ExtendedKalmanFilterTest.cpp | 2 +- .../native/cpp/estimator/KalmanFilterTest.cpp | 6 +- .../native/cpp/estimator/MerweUKFTest.cpp | 9 +- .../test/native/cpp/estimator/S3UKFTest.cpp | 9 +- .../SwerveDrivePoseEstimator3dTest.cpp | 1 - .../SwerveDrivePoseEstimatorTest.cpp | 1 - ...{LinearSystemIdTest.cpp => ModelsTest.cpp} | 116 ++-- .../{plant => }/proto/DCMotorProtoTest.cpp | 2 +- .../{plant => }/struct/DCMotorStructTest.cpp | 2 +- 108 files changed, 1808 insertions(+), 2138 deletions(-) delete mode 100644 wpimath/src/generated/main/java/org/wpilib/math/proto/Plant.java delete mode 100644 wpimath/src/generated/main/native/cpp/wpimath/protobuf/plant.npb.cpp delete mode 100644 wpimath/src/generated/main/native/cpp/wpimath/protobuf/plant.npb.h rename wpimath/src/main/java/org/wpilib/math/system/{plant => }/DCMotor.java (98%) create mode 100644 wpimath/src/main/java/org/wpilib/math/system/Models.java delete mode 100644 wpimath/src/main/java/org/wpilib/math/system/plant/LinearSystemId.java rename wpimath/src/main/java/org/wpilib/math/system/{plant => }/proto/DCMotorProto.java (89%) rename wpimath/src/main/java/org/wpilib/math/system/{plant => }/struct/DCMotorStruct.java (93%) rename wpimath/src/main/native/cpp/system/{plant => }/proto/DCMotorProto.cpp (92%) rename wpimath/src/main/native/cpp/system/{plant => }/struct/DCMotorStruct.cpp (96%) rename wpimath/src/main/native/include/wpi/math/system/{plant => }/DCMotor.hpp (98%) rename wpimath/src/main/native/include/wpi/math/system/{plant/LinearSystemId.hpp => Models.hpp} (61%) rename wpimath/src/main/native/include/wpi/math/system/{plant => }/proto/DCMotorProto.hpp (89%) rename wpimath/src/main/native/include/wpi/math/system/{plant => }/struct/DCMotorStruct.hpp (95%) delete mode 100644 wpimath/src/main/proto/plant.proto delete mode 100644 wpimath/src/main/python/semiwrap/LinearSystemId.yml create mode 100644 wpimath/src/main/python/semiwrap/Models.yml rename wpimath/src/test/java/org/wpilib/math/system/{LinearSystemIDTest.java => ModelsTest.java} (63%) rename wpimath/src/test/java/org/wpilib/math/system/{plant => }/proto/DCMotorProtoTest.java (87%) rename wpimath/src/test/java/org/wpilib/math/system/{plant => }/struct/DCMotorStructTest.java (92%) rename wpimath/src/test/native/cpp/system/{LinearSystemIdTest.cpp => ModelsTest.cpp} (53%) rename wpimath/src/test/native/cpp/system/{plant => }/proto/DCMotorProtoTest.cpp (96%) rename wpimath/src/test/native/cpp/system/{plant => }/struct/DCMotorStructTest.cpp (96%) diff --git a/docs/build.gradle b/docs/build.gradle index dabe4f97a8..4f2b09800b 100644 --- a/docs/build.gradle +++ b/docs/build.gradle @@ -181,8 +181,6 @@ task generateJavaDocs(type: Javadoc) { "-org.wpilib.math.spline.struct," + "-org.wpilib.math.system.proto," + "-org.wpilib.math.system.struct," + - "-org.wpilib.math.system.plant.proto," + - "-org.wpilib.math.system.plant.struct," + "-org.wpilib.math.trajectory.proto," + "-org.wpilib.math.trajectory.struct," + "-org.wpilib.command3.proto," + diff --git a/tools/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/tools/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp index 3dcd9abc98..31d71707cf 100644 --- a/tools/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp +++ b/tools/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp @@ -8,7 +8,7 @@ #include "wpi/math/controller/LinearQuadraticRegulator.hpp" #include "wpi/math/system/LinearSystem.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/sysid/analysis/FeedbackControllerPreset.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/velocity.hpp" @@ -16,13 +16,14 @@ using namespace sysid; -using Kv_t = decltype(1_V / 1_mps); -using Ka_t = decltype(1_V / 1_mps_sq); using Matrix1d = Eigen::Matrix; FeedbackGains sysid::CalculatePositionFeedbackGains( const FeedbackControllerPreset& preset, const LQRParameters& params, double Kv, double Ka) { + using Kv_t = decltype(1_V / 1_mps); + using Ka_t = decltype(1_V / 1_mps_sq); + if (!std::isfinite(Kv) || !std::isfinite(Ka)) { return {0.0, 0.0}; } @@ -43,9 +44,7 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( return {Kv * controller.K(0, 0) * preset.outputConversionFactor, 0.0}; } - auto system = - wpi::math::LinearSystemId::IdentifyPositionSystem( - Kv_t{Kv}, Ka_t{Ka}); + auto system = wpi::math::Models::ElevatorFromSysId(Kv_t{Kv}, Ka_t{Ka}); wpi::math::LinearQuadraticRegulator<2, 1> controller{ system, {params.qp, params.qv}, {params.r}, preset.period}; @@ -61,6 +60,9 @@ FeedbackGains sysid::CalculatePositionFeedbackGains( FeedbackGains sysid::CalculateVelocityFeedbackGains( const FeedbackControllerPreset& preset, const LQRParameters& params, double Kv, double Ka, double encFactor) { + using Kv_t = decltype(1_V / 1_rad_per_s); + using Ka_t = decltype(1_V / 1_rad_per_s_sq); + if (!std::isfinite(Kv) || !std::isfinite(Ka)) { return {0.0, 0.0}; } @@ -72,9 +74,7 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains( return {0.0, 0.0}; } - auto system = - wpi::math::LinearSystemId::IdentifyVelocitySystem( - Kv_t{Kv}, Ka_t{Ka}); + auto system = wpi::math::Models::FlywheelFromSysId(Kv_t{Kv}, Ka_t{Ka}); wpi::math::LinearQuadraticRegulator<1, 1> controller{ system, {params.qv}, {params.r}, preset.period}; controller.LatencyCompensate(system, preset.period, preset.measurementDelay); diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp index a307cbbd3f..4bf418d919 100644 --- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp @@ -7,8 +7,8 @@ #include #include "wpi/math/random/Normal.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/math/system/NumericalIntegration.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" #include "wpi/math/util/StateSpaceUtil.hpp" #include "wpi/system/RobotController.hpp" #include "wpi/util/MathExtras.hpp" @@ -39,7 +39,7 @@ DifferentialDrivetrainSim::DifferentialDrivetrainSim( wpi::units::meter_t wheelRadius, wpi::units::meter_t trackwidth, const std::array& measurementStdDevs) : DifferentialDrivetrainSim( - wpi::math::LinearSystemId::DrivetrainVelocitySystem( + wpi::math::Models::DifferentialDriveFromPhysicalConstants( driveMotor, mass, wheelRadius, trackwidth / 2.0, J, gearing), trackwidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {} diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 07e66e2863..27e2de0019 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -4,8 +4,8 @@ #include "wpi/simulation/ElevatorSim.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/math/system/NumericalIntegration.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" #include "wpi/system/RobotController.hpp" #include "wpi/util/MathExtras.hpp" @@ -33,7 +33,7 @@ ElevatorSim::ElevatorSim(const wpi::math::DCMotor& gearbox, double gearing, wpi::units::meter_t maxHeight, bool simulateGravity, wpi::units::meter_t startingHeight, const std::array& measurementStdDevs) - : ElevatorSim(wpi::math::LinearSystemId::ElevatorSystem( + : ElevatorSim(wpi::math::Models::ElevatorFromPhysicalConstants( gearbox, carriageMass, drumRadius, gearing), gearbox, minHeight, maxHeight, simulateGravity, startingHeight, measurementStdDevs) {} @@ -48,9 +48,9 @@ ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t(1)) kV, wpi::units::meter_t maxHeight, bool simulateGravity, wpi::units::meter_t startingHeight, const std::array& measurementStdDevs) - : ElevatorSim(wpi::math::LinearSystemId::IdentifyPositionSystem(kV, kA), - gearbox, minHeight, maxHeight, simulateGravity, - startingHeight, measurementStdDevs) {} + : ElevatorSim(wpi::math::Models::ElevatorFromSysId(kV, kA), gearbox, + minHeight, maxHeight, simulateGravity, startingHeight, + measurementStdDevs) {} void ElevatorSim::SetState(wpi::units::meter_t position, wpi::units::meters_per_second_t velocity) { diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index d22ba2dacf..7708003746 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -6,8 +6,8 @@ #include +#include "wpi/math/system/Models.hpp" #include "wpi/math/system/NumericalIntegration.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" #include "wpi/system/RobotController.hpp" #include "wpi/units/voltage.hpp" #include "wpi/util/MathExtras.hpp" @@ -38,10 +38,11 @@ SingleJointedArmSim::SingleJointedArmSim( wpi::units::radian_t minAngle, wpi::units::radian_t maxAngle, bool simulateGravity, wpi::units::radian_t startingAngle, const std::array& measurementStdDevs) - : SingleJointedArmSim(wpi::math::LinearSystemId::SingleJointedArmSystem( - gearbox, moi, gearing), - gearbox, gearing, armLength, minAngle, maxAngle, - simulateGravity, startingAngle, measurementStdDevs) {} + : SingleJointedArmSim( + wpi::math::Models::SingleJointedArmFromPhysicalConstants(gearbox, moi, + gearing), + gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity, + startingAngle, measurementStdDevs) {} void SingleJointedArmSim::SetState(wpi::units::radian_t angle, wpi::units::radians_per_second_t velocity) { diff --git a/wpilibc/src/main/native/include/wpi/simulation/DCMotorSim.hpp b/wpilibc/src/main/native/include/wpi/simulation/DCMotorSim.hpp index 91c512e212..8d82290467 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/DCMotorSim.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/DCMotorSim.hpp @@ -4,8 +4,8 @@ #pragma once +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/LinearSystem.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" #include "wpi/simulation/LinearSystemSim.hpp" #include "wpi/units/angle.hpp" #include "wpi/units/angular_acceleration.hpp" @@ -22,12 +22,11 @@ class DCMotorSim : public LinearSystemSim<2, 1, 2> { /** * Creates a simulated DC motor mechanism. * - * @param plant The linear system representing the DC motor. This - * system can be created with wpi::math::LinearSystemId::DCMotorSystem(). If - * wpi::math::LinearSystemId::DCMotorSystem(kV, kA) is used, the distance unit - * must be radians. - * @param gearbox The type of and number of motors in the DC motor - * gearbox. + * @param plant The linear system representing the DC motor. This system can + * be created with wpi::math::Models::ElevatorFromPhysicalConstants(). If + * wpi::math::Models::ElevatorFromSysId(kV, kA) is used, the distance unit + * must be radians. + * @param gearbox The type of and number of motors in the DC motor gearbox. * @param measurementStdDevs The standard deviation of the measurement noise. */ DCMotorSim(const wpi::math::LinearSystem<2, 1, 2>& plant, diff --git a/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp b/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp index 64927e7cca..f1a7f1cb28 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/DifferentialDrivetrainSim.hpp @@ -4,13 +4,15 @@ #pragma once -#include "wpi/math/kinematics/DifferentialDriveKinematics.hpp" +#include "wpi/math/geometry/Pose2d.hpp" +#include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/math/linalg/EigenCore.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/LinearSystem.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" #include "wpi/units/length.hpp" #include "wpi/units/moment_of_inertia.hpp" #include "wpi/units/time.hpp" +#include "wpi/units/velocity.hpp" #include "wpi/units/voltage.hpp" namespace wpi::sim { @@ -21,24 +23,22 @@ class DifferentialDrivetrainSim { * Creates a simulated differential drivetrain. * * @param plant The wpi::math::LinearSystem representing the robot's - * drivetrain. This system can be created with - * wpi::math::LinearSystemId::DrivetrainVelocitySystem() or - * wpi::math::LinearSystemId::IdentifyDrivetrainSystem(). - * @param trackwidth The robot's trackwidth. - * @param driveMotor A wpi::math::DCMotor representing the left side of the - * drivetrain. + * drivetrain. This system can be created with + * wpi::math::Models::DifferentialDriveFromPhysicalConstants() or + * wpi::math::Models::DifferentialDriveFromSysId(). + * @param trackwidth The robot's trackwidth. + * @param driveMotor A wpi::math::DCMotor representing the left side of the + * drivetrain. * @param gearingRatio The gearingRatio ratio of the left side, as output over - * input. This must be the same ratio as the ratio used to - * identify or create the plant. + * input. This must be the same ratio as the ratio used to identify or + * create the plant. * @param wheelRadius The radius of the wheels on the drivetrain, in meters. * @param measurementStdDevs Standard deviations for measurements, in the form - * [x, y, heading, left velocity, right velocity, - * left distance, right distance]ᵀ. Can be omitted - * if no noise is desired. Gyro standard deviations - * of 0.0001 radians, velocity standard deviations - * of 0.05 m/s, and position measurement standard - * deviations of 0.005 meters are a reasonable - * starting point. + * [x, y, heading, left velocity, right velocity, left distance, right + * distance]ᵀ. Can be omitted if no noise is desired. Gyro standard + * deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, + * and position measurement standard deviations of 0.005 meters are a + * reasonable starting point. */ DifferentialDrivetrainSim( wpi::math::LinearSystem<2, 2, 2> plant, wpi::units::meter_t trackwidth, @@ -49,25 +49,22 @@ class DifferentialDrivetrainSim { /** * Creates a simulated differential drivetrain. * - * @param driveMotor A wpi::math::DCMotor representing the left side of the - * drivetrain. - * @param gearing The gearing on the drive between motor and wheel, as - * output over input. This must be the same ratio as the - * ratio used to identify or create the plant. - * @param J The moment of inertia of the drivetrain about its - * center. - * @param mass The mass of the drivebase. + * @param driveMotor A wpi::math::DCMotor representing the left side of the + * drivetrain. + * @param gearing The gearing on the drive between motor and wheel, as output + * over input. This must be the same ratio as the ratio used to identify + * or create the plant. + * @param J The moment of inertia of the drivetrain about its center. + * @param mass The mass of the drivebase. * @param wheelRadius The radius of the wheels on the drivetrain. - * @param trackwidth The robot's trackwidth, or distance between left and - * right wheels. + * @param trackwidth The robot's trackwidth, or distance between left and + * right wheels. * @param measurementStdDevs Standard deviations for measurements, in the form - * [x, y, heading, left velocity, right velocity, - * left distance, right distance]ᵀ. Can be omitted - * if no noise is desired. Gyro standard deviations - * of 0.0001 radians, velocity standard deviations - * of 0.05 m/s, and position measurement standard - * deviations of 0.005 meters are a reasonable - * starting point. + * [x, y, heading, left velocity, right velocity, left distance, right + * distance]ᵀ. Can be omitted if no noise is desired. Gyro standard + * deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, + * and position measurement standard deviations of 0.005 meters are a + * reasonable starting point. */ DifferentialDrivetrainSim( wpi::math::DCMotor driveMotor, double gearing, diff --git a/wpilibc/src/main/native/include/wpi/simulation/ElevatorSim.hpp b/wpilibc/src/main/native/include/wpi/simulation/ElevatorSim.hpp index b04894152e..02b6d362c3 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/ElevatorSim.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/ElevatorSim.hpp @@ -6,7 +6,7 @@ #include -#include "wpi/math/system/plant/DCMotor.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/simulation/LinearSystemSim.hpp" #include "wpi/units/length.hpp" #include "wpi/units/mass.hpp" @@ -31,15 +31,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> { /** * Constructs a simulated elevator mechanism. * - * @param plant The linear system that represents the elevator. - * This system can be created with - * wpi::math::LinearSystemId::ElevatorSystem(). - * @param gearbox The type of and number of motors in your - * elevator gearbox. - * @param minHeight The minimum allowed height of the elevator. - * @param maxHeight The maximum allowed height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. - * @param startingHeight The starting height of the elevator. + * @param plant The linear system that represents the elevator. This system + * can be created with wpi::math::Models::ElevatorFromPhysicalConstants(). + * @param gearbox The type of and number of motors in your elevator gearbox. + * @param minHeight The minimum allowed height of the elevator. + * @param maxHeight The maximum allowed height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingHeight The starting height of the elevator. * @param measurementStdDevs The standard deviation of the measurements. */ ElevatorSim(const wpi::math::LinearSystem<2, 1, 2>& plant, @@ -51,17 +49,15 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> { /** * Constructs a simulated elevator mechanism. * - * @param gearbox The type of and number of motors in your - * elevator gearbox. - * @param gearing The gearing of the elevator (numbers greater - * than 1 represent reductions). - * @param carriageMass The mass of the elevator carriage. - * @param drumRadius The radius of the drum that your cable is - * wrapped around. - * @param minHeight The minimum allowed height of the elevator. - * @param maxHeight The maximum allowed height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. - * @param startingHeight The starting height of the elevator. + * @param gearbox The type of and number of motors in your elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 + * represent reductions). + * @param carriageMass The mass of the elevator carriage. + * @param drumRadius The radius of the drum that your cable is wrapped around. + * @param minHeight The minimum allowed height of the elevator. + * @param maxHeight The maximum allowed height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingHeight The starting height of the elevator. * @param measurementStdDevs The standard deviation of the measurements. */ ElevatorSim(const wpi::math::DCMotor& gearbox, double gearing, @@ -74,14 +70,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> { /** * Constructs a simulated elevator mechanism. * - * @param kV The velocity gain. - * @param kA The acceleration gain. - * @param gearbox The type of and number of motors in your - * elevator gearbox. - * @param minHeight The minimum allowed height of the elevator. - * @param maxHeight The maximum allowed height of the elevator. - * @param simulateGravity Whether gravity should be simulated or not. - * @param startingHeight The starting height of the elevator. + * @param kV The velocity gain. + * @param kA The acceleration gain. + * @param gearbox The type of and number of motors in your elevator gearbox. + * @param minHeight The minimum allowed height of the elevator. + * @param maxHeight The maximum allowed height of the elevator. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingHeight The starting height of the elevator. * @param measurementStdDevs The standard deviation of the measurements. */ template @@ -98,6 +93,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> { /** * Sets the elevator's state. The new position will be limited between the * minimum and maximum allowed heights. + * * @param position The new position * @param velocity The new velocity */ @@ -167,8 +163,8 @@ class ElevatorSim : public LinearSystemSim<2, 1, 2> { * Updates the state estimate of the elevator. * * @param currentXhat The current state estimate. - * @param u The system inputs (voltage). - * @param dt The time difference between controller updates. + * @param u The system inputs (voltage). + * @param dt The time difference between controller updates. */ wpi::math::Vectord<2> UpdateX(const wpi::math::Vectord<2>& currentXhat, const wpi::math::Vectord<1>& u, diff --git a/wpilibc/src/main/native/include/wpi/simulation/FlywheelSim.hpp b/wpilibc/src/main/native/include/wpi/simulation/FlywheelSim.hpp index 07a684fdac..e8f7a1bf04 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/FlywheelSim.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/FlywheelSim.hpp @@ -4,8 +4,8 @@ #pragma once +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/LinearSystem.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" #include "wpi/simulation/LinearSystemSim.hpp" #include "wpi/units/angular_acceleration.hpp" #include "wpi/units/angular_velocity.hpp" @@ -21,12 +21,10 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> { /** * Creates a simulated flywheel mechanism. * - * @param plant The linear system representing the flywheel. This - * system can be created with - * wpi::math::LinearSystemId::FlywheelSystem() or - * wpi::math::LinearSystemId::IdentifyVelocitySystem(). - * @param gearbox The type of and number of motors in the flywheel - * gearbox. + * @param plant The linear system representing the flywheel. This system can + * be created with wpi::math::Models::FlywheelFromPhysicalConstants() or + * wpi::math::Models::FlywheelFromSysId(). + * @param gearbox The type of and number of motors in the flywheel gearbox. * @param measurementStdDevs The standard deviation of the measurement noise. */ FlywheelSim(const wpi::math::LinearSystem<1, 1, 1>& plant, diff --git a/wpilibc/src/main/native/include/wpi/simulation/SingleJointedArmSim.hpp b/wpilibc/src/main/native/include/wpi/simulation/SingleJointedArmSim.hpp index a7be6f6137..3e8e3d68cc 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/SingleJointedArmSim.hpp +++ b/wpilibc/src/main/native/include/wpi/simulation/SingleJointedArmSim.hpp @@ -6,7 +6,7 @@ #include -#include "wpi/math/system/plant/DCMotor.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/simulation/LinearSystemSim.hpp" #include "wpi/units/angle.hpp" #include "wpi/units/length.hpp" @@ -22,17 +22,16 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> { /** * Creates a simulated arm mechanism. * - * @param system The system representing this arm. This system can - * be created with - * wpi::math::LinearSystemId::SingleJointedArmSystem(). - * @param gearbox The type and number of motors on the arm gearbox. - * @param gearing The gear ratio of the arm (numbers greater than 1 - * represent reductions). - * @param armLength The length of the arm. - * @param minAngle The minimum angle that the arm is capable of. - * @param maxAngle The maximum angle that the arm is capable of. - * @param simulateGravity Whether gravity should be simulated or not. - * @param startingAngle The initial position of the arm. + * @param system The system representing this arm. This system can be created + * with wpi::math::Models::SingleJointedArmFromPhysicalConstants(). + * @param gearbox The type and number of motors on the arm gearbox. + * @param gearing The gear ratio of the arm (numbers greater than 1 represent + * reductions). + * @param armLength The length of the arm. + * @param minAngle The minimum angle that the arm is capable of. + * @param maxAngle The maximum angle that the arm is capable of. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingAngle The initial position of the arm. * @param measurementStdDevs The standard deviations of the measurements. */ SingleJointedArmSim(const wpi::math::LinearSystem<2, 1, 2>& system, @@ -46,16 +45,16 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> { /** * Creates a simulated arm mechanism. * - * @param gearbox The type and number of motors on the arm gearbox. - * @param gearing The gear ratio of the arm (numbers greater than 1 - * represent reductions). - * @param moi The moment of inertia of the arm. This can be - * calculated from CAD software. - * @param armLength The length of the arm. - * @param minAngle The minimum angle that the arm is capable of. - * @param maxAngle The maximum angle that the arm is capable of. - * @param simulateGravity Whether gravity should be simulated or not. - * @param startingAngle The initial position of the arm. + * @param gearbox The type and number of motors on the arm gearbox. + * @param gearing The gear ratio of the arm (numbers greater than 1 represent + * reductions). + * @param moi The moment of inertia of the arm. This can be calculated from + * CAD software. + * @param armLength The length of the arm. + * @param minAngle The minimum angle that the arm is capable of. + * @param maxAngle The maximum angle that the arm is capable of. + * @param simulateGravity Whether gravity should be simulated or not. + * @param startingAngle The initial position of the arm. * @param measurementStdDevs The standard deviation of the measurement noise. */ SingleJointedArmSim( diff --git a/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp index cfbab6f8c3..273a7cdf3e 100644 --- a/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DCMotorSimTest.cpp @@ -9,7 +9,7 @@ #include "wpi/hardware/motor/PWMVictorSPX.hpp" #include "wpi/hardware/rotation/Encoder.hpp" #include "wpi/math/controller/PIDController.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/simulation/BatterySim.hpp" #include "wpi/simulation/EncoderSim.hpp" #include "wpi/simulation/RoboRioSim.hpp" @@ -17,7 +17,7 @@ TEST(DCMotorSimTest, VoltageSteadyState) { wpi::math::DCMotor gearbox = wpi::math::DCMotor::NEO(1); - auto plant = wpi::math::LinearSystemId::DCMotorSystem( + auto plant = wpi::math::Models::SingleJointedArmFromPhysicalConstants( wpi::math::DCMotor::NEO(1), wpi::units::kilogram_square_meter_t{0.0005}, 1.0); wpi::sim::DCMotorSim sim{plant, gearbox}; @@ -64,7 +64,7 @@ TEST(DCMotorSimTest, VoltageSteadyState) { TEST(DCMotorSimTest, PositionFeedbackControl) { wpi::math::DCMotor gearbox = wpi::math::DCMotor::NEO(1); - auto plant = wpi::math::LinearSystemId::DCMotorSystem( + auto plant = wpi::math::Models::SingleJointedArmFromPhysicalConstants( wpi::math::DCMotor::NEO(1), wpi::units::kilogram_square_meter_t{0.0005}, 1.0); wpi::sim::DCMotorSim sim{plant, gearbox}; diff --git a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp index 3c89bdf705..76fc3337a8 100644 --- a/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DifferentialDrivetrainSimTest.cpp @@ -9,9 +9,9 @@ #include "wpi/math/controller/LTVUnicycleController.hpp" #include "wpi/math/controller/LinearPlantInversionFeedforward.hpp" #include "wpi/math/kinematics/DifferentialDriveKinematics.hpp" +#include "wpi/math/system/DCMotor.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/math/system/NumericalIntegration.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" #include "wpi/math/trajectory/TrajectoryGenerator.hpp" #include "wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp" #include "wpi/units/current.hpp" @@ -20,7 +20,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) { auto motor = wpi::math::DCMotor::NEO(2); - auto plant = wpi::math::LinearSystemId::DrivetrainVelocitySystem( + auto plant = wpi::math::Models::DifferentialDriveFromPhysicalConstants( motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0); wpi::math::DifferentialDriveKinematics kinematics{24_in}; @@ -73,7 +73,7 @@ TEST(DifferentialDrivetrainSimTest, Convergence) { TEST(DifferentialDrivetrainSimTest, Current) { auto motor = wpi::math::DCMotor::NEO(2); - auto plant = wpi::math::LinearSystemId::DrivetrainVelocitySystem( + auto plant = wpi::math::Models::DifferentialDriveFromPhysicalConstants( motor, 50_kg, 2_in, 12_in, 0.5_kg_sq_m, 1.0); wpi::math::DifferentialDriveKinematics kinematics{24_in}; @@ -100,7 +100,7 @@ TEST(DifferentialDrivetrainSimTest, Current) { TEST(DifferentialDrivetrainSimTest, ModelStability) { auto motor = wpi::math::DCMotor::NEO(2); - auto plant = wpi::math::LinearSystemId::DrivetrainVelocitySystem( + auto plant = wpi::math::Models::DifferentialDriveFromPhysicalConstants( motor, 50_kg, 2_in, 12_in, 2_kg_sq_m, 5.0); wpi::math::DifferentialDriveKinematics kinematics{24_in}; diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp index 72e918ee39..459858eef4 100644 --- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp @@ -9,9 +9,9 @@ #include "wpi/hardware/motor/PWMVictorSPX.hpp" #include "wpi/hardware/rotation/Encoder.hpp" #include "wpi/math/controller/PIDController.hpp" +#include "wpi/math/system/DCMotor.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/math/system/NumericalIntegration.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" #include "wpi/simulation/EncoderSim.hpp" #include "wpi/system/RobotController.hpp" #include "wpi/units/math.hpp" @@ -92,7 +92,7 @@ TEST(ElevatorSimTest, Stability) { } wpi::math::LinearSystem<2, 1, 1> system = - wpi::math::LinearSystemId::ElevatorSystem( + wpi::math::Models::ElevatorFromPhysicalConstants( wpi::math::DCMotor::Vex775Pro(4), 4_kg, 0.5_in, 100) .Slice(0); EXPECT_NEAR_UNITS(wpi::units::meter_t{system.CalculateX( diff --git a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp index 110d51e2d5..abcc0e028a 100644 --- a/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp @@ -8,7 +8,7 @@ #include "wpi/hardware/rotation/Encoder.hpp" #include "wpi/math/controller/PIDController.hpp" #include "wpi/math/controller/SimpleMotorFeedforward.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/simulation/BatterySim.hpp" #include "wpi/simulation/DifferentialDrivetrainSim.hpp" #include "wpi/simulation/ElevatorSim.hpp" @@ -23,8 +23,8 @@ TEST(StateSpaceSimTest, FlywheelSim) { const wpi::math::LinearSystem<1, 1, 1> plant = - wpi::math::LinearSystemId::IdentifyVelocitySystem( - 0.02_V / 1_rad_per_s, 0.01_V / 1_rad_per_s_sq); + wpi::math::Models::FlywheelFromSysId(0.02_V / 1_rad_per_s, + 0.01_V / 1_rad_per_s_sq); wpi::sim::FlywheelSim sim{plant, wpi::math::DCMotor::NEO(2)}; wpi::math::PIDController controller{0.2, 0.0, 0.0}; wpi::math::SimpleMotorFeedforward feedforward{ diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp index ddf0bd99bc..ba2ddc7d06 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp @@ -6,6 +6,10 @@ #include +#include "wpi/math/util/ComputerVisionUtil.hpp" +#include "wpi/smartdashboard/SmartDashboard.hpp" +#include "wpi/system/RobotController.hpp" + Drivetrain::Drivetrain() { m_leftLeader.AddFollower(m_leftFollower); m_rightLeader.AddFollower(m_rightFollower); diff --git a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.hpp b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.hpp index dac2ce98b2..6fe5d997d5 100644 --- a/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.hpp +++ b/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.hpp @@ -16,19 +16,14 @@ #include "wpi/math/estimator/DifferentialDrivePoseEstimator.hpp" #include "wpi/math/geometry/Pose2d.hpp" #include "wpi/math/geometry/Pose3d.hpp" -#include "wpi/math/geometry/Quaternion.hpp" #include "wpi/math/geometry/Transform3d.hpp" #include "wpi/math/kinematics/DifferentialDriveKinematics.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" -#include "wpi/math/util/ComputerVisionUtil.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/nt/DoubleArrayTopic.hpp" #include "wpi/nt/NetworkTableInstance.hpp" #include "wpi/simulation/DifferentialDrivetrainSim.hpp" #include "wpi/simulation/EncoderSim.hpp" #include "wpi/smartdashboard/Field2d.hpp" -#include "wpi/smartdashboard/SmartDashboard.hpp" -#include "wpi/system/RobotController.hpp" -#include "wpi/system/Timer.hpp" #include "wpi/units/angle.hpp" #include "wpi/units/angular_velocity.hpp" #include "wpi/units/length.hpp" @@ -174,7 +169,7 @@ class Drivetrain { wpi::Field2d m_fieldSim; wpi::Field2d m_fieldApproximation; wpi::math::LinearSystem<2, 2, 2> m_drivetrainSystem = - wpi::math::LinearSystemId::IdentifyDrivetrainSystem( + wpi::math::Models::DifferentialDriveFromSysId( 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq); wpi::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ m_drivetrainSystem, kTrackwidth, wpi::math::DCMotor::CIM(2), 8, 2_in}; diff --git a/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp index 091e33c377..cb04e86ef0 100644 --- a/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/FlywheelBangBangController/cpp/Robot.cpp @@ -8,7 +8,7 @@ #include "wpi/hardware/rotation/Encoder.hpp" #include "wpi/math/controller/BangBangController.hpp" #include "wpi/math/controller/SimpleMotorFeedforward.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/simulation/EncoderSim.hpp" #include "wpi/simulation/FlywheelSim.hpp" #include "wpi/smartdashboard/SmartDashboard.hpp" @@ -96,7 +96,7 @@ class Robot : public wpi::TimedRobot { wpi::math::DCMotor m_gearbox = wpi::math::DCMotor::NEO(1); wpi::math::LinearSystem<1, 1, 1> m_plant{ - wpi::math::LinearSystemId::FlywheelSystem( + wpi::math::Models::FlywheelFromPhysicalConstants( m_gearbox, kFlywheelMomentOfInertia, kFlywheelGearing)}; wpi::sim::FlywheelSim m_flywheelSim{m_plant, m_gearbox}; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.hpp b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.hpp index 5d606f7b76..074984c9f7 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.hpp +++ b/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.hpp @@ -14,7 +14,6 @@ #include "wpi/math/estimator/MecanumDrivePoseEstimator.hpp" #include "wpi/math/geometry/Translation2d.hpp" #include "wpi/math/kinematics/MecanumDriveKinematics.hpp" -#include "wpi/math/kinematics/MecanumDriveOdometry.hpp" #include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp" /** diff --git a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.hpp b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.hpp index 7e73fdbfeb..b53063b52b 100644 --- a/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.hpp +++ b/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.hpp @@ -13,12 +13,11 @@ #include "wpi/math/controller/SimpleMotorFeedforward.hpp" #include "wpi/math/kinematics/DifferentialDriveKinematics.hpp" #include "wpi/math/kinematics/DifferentialDriveOdometry.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/simulation/DifferentialDrivetrainSim.hpp" #include "wpi/simulation/EncoderSim.hpp" #include "wpi/smartdashboard/Field2d.hpp" #include "wpi/smartdashboard/SmartDashboard.hpp" -#include "wpi/units/angle.hpp" #include "wpi/units/angular_velocity.hpp" #include "wpi/units/length.hpp" #include "wpi/units/velocity.hpp" @@ -104,7 +103,7 @@ class Drivetrain { wpi::sim::EncoderSim m_rightEncoderSim{m_rightEncoder}; wpi::Field2d m_fieldSim; wpi::math::LinearSystem<2, 2, 2> m_drivetrainSystem = - wpi::math::LinearSystemId::IdentifyDrivetrainSystem( + wpi::math::Models::DifferentialDriveFromSysId( 1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq); wpi::sim::DifferentialDrivetrainSim m_drivetrainSimulator{ m_drivetrainSystem, kTrackwidth, wpi::math::DCMotor::CIM(2), 8, 2_in}; diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp index 48dfdfa54e..6b16f1e88d 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp @@ -4,16 +4,15 @@ #include -#include "wpi/drive/DifferentialDrive.hpp" #include "wpi/driverstation/XboxController.hpp" #include "wpi/framework/TimedRobot.hpp" #include "wpi/hardware/motor/PWMSparkMax.hpp" #include "wpi/hardware/rotation/Encoder.hpp" #include "wpi/math/controller/LinearQuadraticRegulator.hpp" #include "wpi/math/estimator/KalmanFilter.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/LinearSystemLoop.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/math/trajectory/TrapezoidProfile.hpp" #include "wpi/units/angle.hpp" #include "wpi/units/moment_of_inertia.hpp" @@ -47,7 +46,7 @@ class Robot : public wpi::TimedRobot { // Inputs (what we can "put in"): [voltage], in volts. // Outputs (what we can measure): [position], in radians. wpi::math::LinearSystem<2, 1, 1> m_armPlant = - wpi::math::LinearSystemId::SingleJointedArmSystem( + wpi::math::Models::SingleJointedArmFromPhysicalConstants( wpi::math::DCMotor::NEO(2), kArmMOI, kArmGearing) .Slice(0); diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp index a11228f455..b94574551a 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp @@ -4,16 +4,15 @@ #include -#include "wpi/drive/DifferentialDrive.hpp" #include "wpi/driverstation/XboxController.hpp" #include "wpi/framework/TimedRobot.hpp" #include "wpi/hardware/motor/PWMSparkMax.hpp" #include "wpi/hardware/rotation/Encoder.hpp" #include "wpi/math/controller/LinearQuadraticRegulator.hpp" #include "wpi/math/estimator/KalmanFilter.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/LinearSystemLoop.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/math/trajectory/TrapezoidProfile.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/length.hpp" @@ -44,7 +43,7 @@ class Robot : public wpi::TimedRobot { // Inputs (what we can "put in"): [voltage], in volts. // Outputs (what we can measure): [position], in meters. wpi::math::LinearSystem<2, 1, 1> m_elevatorPlant = - wpi::math::LinearSystemId::ElevatorSystem( + wpi::math::Models::ElevatorFromPhysicalConstants( wpi::math::DCMotor::NEO(2), kCarriageMass, kDrumRadius, kGearRatio) .Slice(0); diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp index 3733639296..8174767a05 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp @@ -4,17 +4,15 @@ #include -#include "wpi/drive/DifferentialDrive.hpp" -#include "wpi/driverstation/DriverStation.hpp" #include "wpi/driverstation/XboxController.hpp" #include "wpi/framework/TimedRobot.hpp" #include "wpi/hardware/motor/PWMSparkMax.hpp" #include "wpi/hardware/rotation/Encoder.hpp" #include "wpi/math/controller/LinearQuadraticRegulator.hpp" #include "wpi/math/estimator/KalmanFilter.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/LinearSystemLoop.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/units/angular_velocity.hpp" /** @@ -42,9 +40,9 @@ class Robot : public wpi::TimedRobot { // Inputs (what we can "put in"): [voltage], in volts. // Outputs (what we can measure): [velocity], in radians per second. wpi::math::LinearSystem<1, 1, 1> m_flywheelPlant = - wpi::math::LinearSystemId::FlywheelSystem(wpi::math::DCMotor::NEO(2), - kFlywheelMomentOfInertia, - kFlywheelGearing); + wpi::math::Models::FlywheelFromPhysicalConstants( + wpi::math::DCMotor::NEO(2), kFlywheelMomentOfInertia, + kFlywheelGearing); // The observer fuses our encoder data and voltage inputs to reject noise. wpi::math::KalmanFilter<1, 1, 1> m_observer{ diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp index 49db286d15..54aa0cb832 100644 --- a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp @@ -4,8 +4,6 @@ #include -#include "wpi/drive/DifferentialDrive.hpp" -#include "wpi/driverstation/DriverStation.hpp" #include "wpi/driverstation/XboxController.hpp" #include "wpi/framework/TimedRobot.hpp" #include "wpi/hardware/motor/PWMSparkMax.hpp" @@ -13,8 +11,7 @@ #include "wpi/math/controller/LinearQuadraticRegulator.hpp" #include "wpi/math/estimator/KalmanFilter.hpp" #include "wpi/math/system/LinearSystemLoop.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" /** * This is a sample program to demonstrate how to use a state-space controller @@ -42,8 +39,7 @@ class Robot : public wpi::TimedRobot { // // The Kv and Ka constants are found using the FRC Characterization toolsuite. wpi::math::LinearSystem<1, 1, 1> m_flywheelPlant = - wpi::math::LinearSystemId::IdentifyVelocitySystem( - kFlywheelKv, kFlywheelKa); + wpi::math::Models::FlywheelFromSysId(kFlywheelKv, kFlywheelKa); // The observer fuses our encoder data and voltage inputs to reject noise. wpi::math::KalmanFilter<1, 1, 1> m_observer{ diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.hpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.hpp index 3ee387fd37..b8e866cc9a 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.hpp +++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.hpp @@ -11,7 +11,6 @@ #include "wpi/math/estimator/SwerveDrivePoseEstimator.hpp" #include "wpi/math/geometry/Translation2d.hpp" #include "wpi/math/kinematics/SwerveDriveKinematics.hpp" -#include "wpi/math/kinematics/SwerveDriveOdometry.hpp" /** * Represents a swerve drive style drivetrain. diff --git a/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp index 64e0d7b0b6..677a8aabcc 100644 --- a/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp +++ b/wpilibcExamples/src/test/cpp/examples/PotentiometerPID/cpp/PotentiometerPIDTest.cpp @@ -10,7 +10,7 @@ #include "Robot.hpp" #include "wpi/hal/DriverStationTypes.h" #include "wpi/hal/simulation/MockHooks.h" -#include "wpi/math/system/plant/DCMotor.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/simulation/AnalogInputSim.hpp" #include "wpi/simulation/DriverStationSim.hpp" #include "wpi/simulation/ElevatorSim.hpp" diff --git a/wpilibj/src/main/java/org/wpilib/simulation/DCMotorSim.java b/wpilibj/src/main/java/org/wpilib/simulation/DCMotorSim.java index 35024ee79c..a1e68dbf3d 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/DCMotorSim.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/DCMotorSim.java @@ -7,8 +7,8 @@ package org.wpilib.simulation; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; -import org.wpilib.math.system.plant.DCMotor; import org.wpilib.system.RobotController; /** Represents a simulated DC motor mechanism. */ @@ -26,10 +26,9 @@ public class DCMotorSim extends LinearSystemSim { * Creates a simulated DC motor mechanism. * * @param plant The linear system representing the DC motor. This system can be created with - * {@link org.wpilib.math.system.plant.LinearSystemId#createDCMotorSystem(DCMotor, double, - * double)} or {@link org.wpilib.math.system.plant.LinearSystemId#createDCMotorSystem(double, - * double)}. If {@link org.wpilib.math.system.plant.LinearSystemId#createDCMotorSystem(double, - * double)} is used, the distance unit must be radians. + * {@link org.wpilib.math.system.Models#singleJointedArmFromPhysicalConstants(DCMotor, double, + * double)} or {@link org.wpilib.math.system.Models#singleJointedArmFromSysId(double, + * double)}. * @param gearbox The type of and number of motors in the DC motor gearbox. * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no * noise is desired. If present must have 2 elements. The first element is for position. The diff --git a/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java b/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java index c5371a3035..89bc82521e 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/DifferentialDrivetrainSim.java @@ -12,10 +12,10 @@ import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; import org.wpilib.math.numbers.N7; import org.wpilib.math.random.Normal; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; +import org.wpilib.math.system.Models; import org.wpilib.math.system.NumericalIntegration; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; import org.wpilib.math.util.Nat; import org.wpilib.math.util.StateSpaceUtil; import org.wpilib.math.util.Units; @@ -77,7 +77,7 @@ public class DifferentialDrivetrainSim { double trackwidth, Matrix measurementStdDevs) { this( - LinearSystemId.createDrivetrainVelocitySystem( + Models.differentialDriveFromPhysicalConstants( driveMotor, mass, wheelRadius, trackwidth / 2.0, j, gearing), driveMotor, gearing, @@ -91,10 +91,9 @@ public class DifferentialDrivetrainSim { * * @param plant The {@link LinearSystem} representing the robot's drivetrain. This system can be * created with {@link - * org.wpilib.math.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor, double, + * org.wpilib.math.system.Models#differentialDriveFromPhysicalConstants(DCMotor, double, * double, double, double, double)} or {@link - * org.wpilib.math.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double, - * double, double)}. + * org.wpilib.math.system.Models#differentialDriveFromSysId(double, double, double, double)}. * @param driveMotor A {@link DCMotor} representing the drivetrain. * @param gearing The gearingRatio ratio of the robot, as output over input. This must be the same * ratio as the ratio used to identify or create the drivetrainPlant. diff --git a/wpilibj/src/main/java/org/wpilib/simulation/ElevatorSim.java b/wpilibj/src/main/java/org/wpilib/simulation/ElevatorSim.java index f679ab04a9..ee5daa8fcf 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/ElevatorSim.java @@ -8,10 +8,10 @@ import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; +import org.wpilib.math.system.Models; import org.wpilib.math.system.NumericalIntegration; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; import org.wpilib.system.RobotController; /** Represents a simulated elevator mechanism. */ @@ -32,8 +32,8 @@ public class ElevatorSim extends LinearSystemSim { * Creates a simulated elevator mechanism. * * @param plant The linear system that represents the elevator. This system can be created with - * {@link org.wpilib.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, - * double, double)}. + * {@link org.wpilib.math.system.Models#elevatorFromPhysicalConstants(DCMotor, double, double, + * double)}. * @param gearbox The type of and number of motors in the elevator gearbox. * @param minHeight The min allowable height of the elevator in meters. * @param maxHeight The max allowable height of the elevator in meters. @@ -83,7 +83,7 @@ public class ElevatorSim extends LinearSystemSim { double startingHeight, double... measurementStdDevs) { this( - LinearSystemId.identifyPositionSystem(kV, kA), + Models.elevatorFromSysId(kV, kA), gearbox, minHeight, maxHeight, @@ -117,7 +117,7 @@ public class ElevatorSim extends LinearSystemSim { double startingHeight, double... measurementStdDevs) { this( - LinearSystemId.createElevatorSystem(gearbox, carriageMass, drumRadius, gearing), + Models.elevatorFromPhysicalConstants(gearbox, carriageMass, drumRadius, gearing), gearbox, minHeight, maxHeight, diff --git a/wpilibj/src/main/java/org/wpilib/simulation/FlywheelSim.java b/wpilibj/src/main/java/org/wpilib/simulation/FlywheelSim.java index 84e4b159de..c56dba67dd 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/FlywheelSim.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/FlywheelSim.java @@ -6,9 +6,9 @@ package org.wpilib.simulation; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.system.RobotController; /** Represents a simulated flywheel mechanism. */ @@ -26,9 +26,8 @@ public class FlywheelSim extends LinearSystemSim { * Creates a simulated flywheel mechanism. * * @param plant The linear system that represents the flywheel. Use either {@link - * LinearSystemId#createFlywheelSystem(DCMotor, double, double)} if using physical constants - * or {@link LinearSystemId#identifyVelocitySystem(double, double)} if using system - * characterization. + * Models#flywheelFromPhysicalConstants(DCMotor, double, double)} if using physical constants + * or {@link Models#flywheelFromSysId(double, double)} if using system characterization. * @param gearbox The type of and number of motors in the flywheel gearbox. * @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no * noise is desired. If present must have 1 element for velocity. diff --git a/wpilibj/src/main/java/org/wpilib/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/org/wpilib/simulation/SingleJointedArmSim.java index f210f422c5..dfaf06725a 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/SingleJointedArmSim.java +++ b/wpilibj/src/main/java/org/wpilib/simulation/SingleJointedArmSim.java @@ -8,10 +8,10 @@ import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; +import org.wpilib.math.system.Models; import org.wpilib.math.system.NumericalIntegration; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; import org.wpilib.system.RobotController; /** Represents a simulated single jointed arm mechanism. */ @@ -38,7 +38,7 @@ public class SingleJointedArmSim extends LinearSystemSim { * Creates a simulated arm mechanism. * * @param plant The linear system that represents the arm. This system can be created with {@link - * org.wpilib.math.system.plant.LinearSystemId#createSingleJointedArmSystem(DCMotor, double, + * org.wpilib.math.system.Models#singleJointedArmFromPhysicalConstants(DCMotor, double, * double)}. * @param gearbox The type of and number of motors in the arm gearbox. * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). @@ -97,7 +97,7 @@ public class SingleJointedArmSim extends LinearSystemSim { double startingAngleRads, double... measurementStdDevs) { this( - LinearSystemId.createSingleJointedArmSystem(gearbox, j, gearing), + Models.singleJointedArmFromPhysicalConstants(gearbox, j, gearing), gearbox, gearing, armLength, diff --git a/wpilibj/src/test/java/org/wpilib/simulation/DCMotorSimTest.java b/wpilibj/src/test/java/org/wpilib/simulation/DCMotorSimTest.java index 9ccaa7db5f..c3a66dbaa3 100644 --- a/wpilibj/src/test/java/org/wpilib/simulation/DCMotorSimTest.java +++ b/wpilibj/src/test/java/org/wpilib/simulation/DCMotorSimTest.java @@ -12,9 +12,9 @@ import org.wpilib.hardware.rotation.Encoder; import org.wpilib.math.controller.PIDController; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.system.RobotController; class DCMotorSimTest { @@ -23,7 +23,8 @@ class DCMotorSimTest { RoboRioSim.resetData(); DCMotor gearbox = DCMotor.getNEO(1); - LinearSystem plant = LinearSystemId.createDCMotorSystem(gearbox, 0.0005, 1); + LinearSystem plant = + Models.singleJointedArmFromPhysicalConstants(gearbox, 0.0005, 1); DCMotorSim sim = new DCMotorSim(plant, gearbox); try (var motor = new PWMVictorSPX(0); @@ -64,7 +65,8 @@ class DCMotorSimTest { RoboRioSim.resetData(); DCMotor gearbox = DCMotor.getNEO(1); - LinearSystem plant = LinearSystemId.createDCMotorSystem(gearbox, 0.0005, 1); + LinearSystem plant = + Models.singleJointedArmFromPhysicalConstants(gearbox, 0.0005, 1); DCMotorSim sim = new DCMotorSim(plant, gearbox); try (var motor = new PWMVictorSPX(0); diff --git a/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java index c0f2ec3c09..549dbcd333 100644 --- a/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java +++ b/wpilibj/src/test/java/org/wpilib/simulation/DifferentialDrivetrainSimTest.java @@ -19,9 +19,9 @@ import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.linalg.Vector; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N7; +import org.wpilib.math.system.DCMotor; +import org.wpilib.math.system.Models; import org.wpilib.math.system.NumericalIntegration; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; import org.wpilib.math.trajectory.TrajectoryConfig; import org.wpilib.math.trajectory.TrajectoryGenerator; import org.wpilib.math.trajectory.constraint.DifferentialDriveKinematicsConstraint; @@ -33,7 +33,7 @@ class DifferentialDrivetrainSimTest { void testConvergence() { var motor = DCMotor.getNEO(2); var plant = - LinearSystemId.createDrivetrainVelocitySystem( + Models.differentialDriveFromPhysicalConstants( motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5, 1.0); var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); @@ -97,7 +97,7 @@ class DifferentialDrivetrainSimTest { void testCurrent() { var motor = DCMotor.getNEO(2); var plant = - LinearSystemId.createDrivetrainVelocitySystem( + Models.differentialDriveFromPhysicalConstants( motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5, 1.0); var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); var sim = @@ -127,7 +127,7 @@ class DifferentialDrivetrainSimTest { void testModelStability() { var motor = DCMotor.getNEO(2); var plant = - LinearSystemId.createDrivetrainVelocitySystem( + Models.differentialDriveFromPhysicalConstants( motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 2.0, 5.0); var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24)); diff --git a/wpilibj/src/test/java/org/wpilib/simulation/ElevatorSimTest.java b/wpilibj/src/test/java/org/wpilib/simulation/ElevatorSimTest.java index e6d95289c1..bea2d4a21a 100644 --- a/wpilibj/src/test/java/org/wpilib/simulation/ElevatorSimTest.java +++ b/wpilibj/src/test/java/org/wpilib/simulation/ElevatorSimTest.java @@ -12,8 +12,8 @@ import org.wpilib.hardware.motor.PWMVictorSPX; import org.wpilib.hardware.rotation.Encoder; import org.wpilib.math.controller.PIDController; import org.wpilib.math.linalg.VecBuilder; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.DCMotor; +import org.wpilib.math.system.Models; import org.wpilib.math.util.Units; import org.wpilib.system.RobotController; @@ -117,7 +117,7 @@ class ElevatorSimTest { } var system = - LinearSystemId.createElevatorSystem( + Models.elevatorFromPhysicalConstants( DCMotor.getVex775Pro(4), 4, Units.inchesToMeters(0.5), 100); assertEquals( system.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0), diff --git a/wpilibj/src/test/java/org/wpilib/simulation/SingleJointedArmSimTest.java b/wpilibj/src/test/java/org/wpilib/simulation/SingleJointedArmSimTest.java index 29c0f61125..2f1b40215b 100644 --- a/wpilibj/src/test/java/org/wpilib/simulation/SingleJointedArmSimTest.java +++ b/wpilibj/src/test/java/org/wpilib/simulation/SingleJointedArmSimTest.java @@ -8,7 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; import org.wpilib.math.linalg.VecBuilder; -import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.util.Units; class SingleJointedArmSimTest { diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/armsimulation/subsystems/Arm.java b/wpilibjExamples/src/main/java/org/wpilib/examples/armsimulation/subsystems/Arm.java index 6546281381..cc0acfb0f7 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/armsimulation/subsystems/Arm.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/armsimulation/subsystems/Arm.java @@ -8,7 +8,7 @@ import org.wpilib.examples.armsimulation.Constants; import org.wpilib.hardware.motor.PWMSparkMax; import org.wpilib.hardware.rotation.Encoder; import org.wpilib.math.controller.PIDController; -import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.util.Units; import org.wpilib.simulation.BatterySim; import org.wpilib.simulation.EncoderSim; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java index df1a2ad6d3..c0084c47ca 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/differentialdriveposeestimator/Drivetrain.java @@ -21,9 +21,9 @@ import org.wpilib.math.kinematics.DifferentialDriveKinematics; import org.wpilib.math.kinematics.DifferentialDriveWheelSpeeds; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.math.util.ComputerVisionUtil; import org.wpilib.math.util.Units; import org.wpilib.networktables.DoubleArrayEntry; @@ -93,7 +93,7 @@ public class Drivetrain { private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder); private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder); private final LinearSystem m_drivetrainSystem = - LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + Models.differentialDriveFromSysId(1.98, 0.2, 1.5, 0.3); private final DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim( m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null); diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialsimulation/subsystems/Elevator.java index c77f3cee29..f9e2acd0f6 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/elevatorexponentialsimulation/subsystems/Elevator.java @@ -9,7 +9,7 @@ import org.wpilib.hardware.motor.PWMSparkMax; import org.wpilib.hardware.rotation.Encoder; import org.wpilib.math.controller.ElevatorFeedforward; import org.wpilib.math.controller.PIDController; -import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.trajectory.ExponentialProfile; import org.wpilib.math.util.Units; import org.wpilib.simulation.BatterySim; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/elevatorsimulation/subsystems/Elevator.java b/wpilibjExamples/src/main/java/org/wpilib/examples/elevatorsimulation/subsystems/Elevator.java index 4e5f550324..3c9a9d0b84 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/elevatorsimulation/subsystems/Elevator.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/elevatorsimulation/subsystems/Elevator.java @@ -9,7 +9,7 @@ import org.wpilib.hardware.motor.PWMSparkMax; import org.wpilib.hardware.rotation.Encoder; import org.wpilib.math.controller.ElevatorFeedforward; import org.wpilib.math.controller.ProfiledPIDController; -import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.trajectory.TrapezoidProfile; import org.wpilib.simulation.BatterySim; import org.wpilib.simulation.ElevatorSim; diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/flywheelbangbangcontroller/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/flywheelbangbangcontroller/Robot.java index 32c55a6335..47f67945f9 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/flywheelbangbangcontroller/Robot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/flywheelbangbangcontroller/Robot.java @@ -11,9 +11,9 @@ import org.wpilib.hardware.rotation.Encoder; import org.wpilib.math.controller.BangBangController; import org.wpilib.math.controller.SimpleMotorFeedforward; import org.wpilib.math.numbers.N1; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.math.util.Units; import org.wpilib.simulation.EncoderSim; import org.wpilib.simulation.FlywheelSim; @@ -60,7 +60,7 @@ public class Robot extends TimedRobot { private final DCMotor m_gearbox = DCMotor.getNEO(1); private final LinearSystem m_plant = - LinearSystemId.createFlywheelSystem(m_gearbox, kFlywheelGearing, kFlywheelMomentOfInertia); + Models.flywheelFromPhysicalConstants(m_gearbox, kFlywheelGearing, kFlywheelMomentOfInertia); private final FlywheelSim m_flywheelSim = new FlywheelSim(m_plant, m_gearbox); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/simpledifferentialdrivesimulation/Drivetrain.java b/wpilibjExamples/src/main/java/org/wpilib/examples/simpledifferentialdrivesimulation/Drivetrain.java index 04e2e434fc..ce5685d26d 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/simpledifferentialdrivesimulation/Drivetrain.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/simpledifferentialdrivesimulation/Drivetrain.java @@ -15,9 +15,9 @@ import org.wpilib.math.kinematics.DifferentialDriveKinematics; import org.wpilib.math.kinematics.DifferentialDriveOdometry; import org.wpilib.math.kinematics.DifferentialDriveWheelSpeeds; import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.simulation.DifferentialDrivetrainSim; import org.wpilib.simulation.EncoderSim; import org.wpilib.smartdashboard.Field2d; @@ -62,7 +62,7 @@ public class Drivetrain { private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder); private final Field2d m_fieldSim = new Field2d(); private final LinearSystem m_drivetrainSystem = - LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3); + Models.differentialDriveFromSysId(1.98, 0.2, 1.5, 0.3); private final DifferentialDrivetrainSim m_drivetrainSimulator = new DifferentialDrivetrainSim( m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackwidth, kWheelRadius, null); diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/statespacearm/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/statespacearm/Robot.java index a932ee8b8c..dbc9fd9124 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/statespacearm/Robot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/statespacearm/Robot.java @@ -13,10 +13,10 @@ import org.wpilib.math.estimator.KalmanFilter; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; import org.wpilib.math.system.LinearSystemLoop; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.math.trajectory.TrapezoidProfile; import org.wpilib.math.util.Nat; import org.wpilib.math.util.Units; @@ -53,7 +53,7 @@ public class Robot extends TimedRobot { // Inputs (what we can "put in"): [voltage], in volts. // Outputs (what we can measure): [position], in radians. private final LinearSystem m_armPlant = - LinearSystemId.createSingleJointedArmSystem(DCMotor.getNEO(2), kArmMOI, kArmGearing); + Models.singleJointedArmFromPhysicalConstants(DCMotor.getNEO(2), kArmMOI, kArmGearing); // The observer fuses our encoder data and voltage inputs to reject noise. @SuppressWarnings("unchecked") diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceelevator/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceelevator/Robot.java index da96742f21..47ff7c36a3 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceelevator/Robot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceelevator/Robot.java @@ -13,10 +13,10 @@ import org.wpilib.math.estimator.KalmanFilter; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; import org.wpilib.math.system.LinearSystemLoop; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.math.trajectory.TrapezoidProfile; import org.wpilib.math.util.Nat; import org.wpilib.math.util.Units; @@ -58,7 +58,7 @@ public class Robot extends TimedRobot { This elevator is driven by two NEO motors. */ private final LinearSystem m_elevatorPlant = - LinearSystemId.createElevatorSystem( + Models.elevatorFromPhysicalConstants( DCMotor.getNEO(2), kCarriageMass, kDrumRadius, kElevatorGearing); // The observer fuses our encoder data and voltage inputs to reject noise. diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheel/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheel/Robot.java index 01a02729a5..5ea959a7e8 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheel/Robot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheel/Robot.java @@ -12,10 +12,10 @@ import org.wpilib.math.controller.LinearQuadraticRegulator; import org.wpilib.math.estimator.KalmanFilter; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; import org.wpilib.math.system.LinearSystemLoop; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.math.util.Nat; import org.wpilib.math.util.Units; @@ -42,7 +42,7 @@ public class Robot extends TimedRobot { // Inputs (what we can "put in"): [voltage], in volts. // Outputs (what we can measure): [velocity], in radians per second. private final LinearSystem m_flywheelPlant = - LinearSystemId.createFlywheelSystem( + Models.flywheelFromPhysicalConstants( DCMotor.getNEO(2), kFlywheelMomentOfInertia, kFlywheelGearing); // The observer fuses our encoder data and voltage inputs to reject noise. diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheelsysid/Robot.java b/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheelsysid/Robot.java index a54d056aee..9f2919f145 100644 --- a/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheelsysid/Robot.java +++ b/wpilibjExamples/src/main/java/org/wpilib/examples/statespaceflywheelsysid/Robot.java @@ -14,7 +14,7 @@ import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.system.LinearSystem; import org.wpilib.math.system.LinearSystemLoop; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.math.util.Nat; import org.wpilib.math.util.Units; @@ -43,7 +43,7 @@ public class Robot extends TimedRobot { // // The Kv and Ka constants are found using the FRC Characterization toolsuite. private final LinearSystem m_flywheelPlant = - LinearSystemId.identifyVelocitySystem(kFlywheelKv, kFlywheelKa); + Models.flywheelFromSysId(kFlywheelKv, kFlywheelKa); // The observer fuses our encoder data and voltage inputs to reject noise. private final KalmanFilter m_observer = diff --git a/wpilibjExamples/src/test/java/org/wpilib/examples/potentiometerpid/PotentiometerPIDTest.java b/wpilibjExamples/src/test/java/org/wpilib/examples/potentiometerpid/PotentiometerPIDTest.java index c8ecab7b60..df510b8e4f 100644 --- a/wpilibjExamples/src/test/java/org/wpilib/examples/potentiometerpid/PotentiometerPIDTest.java +++ b/wpilibjExamples/src/test/java/org/wpilib/examples/potentiometerpid/PotentiometerPIDTest.java @@ -14,7 +14,7 @@ import org.junit.jupiter.api.parallel.ResourceLock; import org.wpilib.hardware.hal.HAL; import org.wpilib.hardware.hal.HAL.SimPeriodicBeforeCallback; import org.wpilib.hardware.hal.RobotMode; -import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.util.Units; import org.wpilib.simulation.AnalogInputSim; import org.wpilib.simulation.DriverStationSim; diff --git a/wpimath/robotpy_pybind_build_info.bzl b/wpimath/robotpy_pybind_build_info.bzl index 028636e51e..f370dfdc89 100644 --- a/wpimath/robotpy_pybind_build_info.bzl +++ b/wpimath/robotpy_pybind_build_info.bzl @@ -889,6 +889,16 @@ def wpimath_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu ("wpi::math::SplineParameterizer", "wpi__math__SplineParameterizer.hpp"), ], ), + struct( + class_name = "DCMotor", + yml_file = "semiwrap/DCMotor.yml", + header_root = "$(execpath :robotpy-native-wpimath.copy_headers)", + header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/system/DCMotor.hpp", + tmpl_class_names = [], + trampolines = [ + ("wpi::math::DCMotor", "wpi__math__DCMotor.hpp"), + ], + ), struct( class_name = "LinearSystem", yml_file = "semiwrap/LinearSystem.yml", @@ -929,23 +939,13 @@ def wpimath_extension(srcs = [], header_to_dat_deps = [], extra_hdrs = [], inclu ], ), struct( - class_name = "DCMotor", - yml_file = "semiwrap/DCMotor.yml", + class_name = "Models", + yml_file = "semiwrap/Models.yml", header_root = "$(execpath :robotpy-native-wpimath.copy_headers)", - header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/system/plant/DCMotor.hpp", + header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/system/Models.hpp", tmpl_class_names = [], trampolines = [ - ("wpi::math::DCMotor", "wpi__math__DCMotor.hpp"), - ], - ), - struct( - class_name = "LinearSystemId", - yml_file = "semiwrap/LinearSystemId.yml", - header_root = "$(execpath :robotpy-native-wpimath.copy_headers)", - header_file = "$(execpath :robotpy-native-wpimath.copy_headers)/wpi/math/system/plant/LinearSystemId.hpp", - tmpl_class_names = [], - trampolines = [ - ("wpi::math::LinearSystemId", "wpi__math__LinearSystemId.hpp"), + ("wpi::math::Models", "wpi__math__Models.hpp"), ], ), struct( diff --git a/wpimath/src/generated/main/java/org/wpilib/math/proto/Plant.java b/wpimath/src/generated/main/java/org/wpilib/math/proto/Plant.java deleted file mode 100644 index ed3f08adf5..0000000000 --- a/wpimath/src/generated/main/java/org/wpilib/math/proto/Plant.java +++ /dev/null @@ -1,621 +0,0 @@ -// 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. -// Code generated by protocol buffer compiler. Do not edit! -package org.wpilib.math.proto; - -import java.io.IOException; -import us.hebi.quickbuf.Descriptors; -import us.hebi.quickbuf.FieldName; -import us.hebi.quickbuf.InvalidProtocolBufferException; -import us.hebi.quickbuf.JsonSink; -import us.hebi.quickbuf.JsonSource; -import us.hebi.quickbuf.MessageFactory; -import us.hebi.quickbuf.ProtoMessage; -import us.hebi.quickbuf.ProtoSink; -import us.hebi.quickbuf.ProtoSource; -import us.hebi.quickbuf.ProtoUtil; -import us.hebi.quickbuf.RepeatedByte; - -public final class Plant { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(607, - "CgtwbGFudC5wcm90bxIJd3BpLnByb3RvIsQBCg9Qcm90b2J1ZkRDTW90b3ISJwoPbm9taW5hbF92b2x0" + - "YWdlGAEgASgBUg5ub21pbmFsVm9sdGFnZRIhCgxzdGFsbF90b3JxdWUYAiABKAFSC3N0YWxsVG9ycXVl" + - "EiMKDXN0YWxsX2N1cnJlbnQYAyABKAFSDHN0YWxsQ3VycmVudBIhCgxmcmVlX2N1cnJlbnQYBCABKAFS" + - "C2ZyZWVDdXJyZW50Eh0KCmZyZWVfc3BlZWQYBSABKAFSCWZyZWVTcGVlZEIXChVvcmcud3BpbGliLm1h" + - "dGgucHJvdG9K3AIKBhIEAAAMAQoICgEMEgMAABIKCAoBAhIDAgASCggKAQgSAwQALgoJCgIIARIDBAAu" + - "CgoKAgQAEgQGAAwBCgoKAwQAARIDBggXCgsKBAQAAgASAwcCHQoMCgUEAAIABRIDBwIICgwKBQQAAgAB" + - "EgMHCRgKDAoFBAACAAMSAwcbHAoLCgQEAAIBEgMIAhoKDAoFBAACAQUSAwgCCAoMCgUEAAIBARIDCAkV" + - "CgwKBQQAAgEDEgMIGBkKCwoEBAACAhIDCQIbCgwKBQQAAgIFEgMJAggKDAoFBAACAgESAwkJFgoMCgUE" + - "AAICAxIDCRkaCgsKBAQAAgMSAwoCGgoMCgUEAAIDBRIDCgIICgwKBQQAAgMBEgMKCRUKDAoFBAACAwMS" + - "AwoYGQoLCgQEAAIEEgMLAhgKDAoFBAACBAUSAwsCCAoMCgUEAAIEARIDCwkTCgwKBQQAAgQDEgMLFhdi" + - "BnByb3RvMw=="); - - static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("plant.proto", "wpi.proto", descriptorData); - - static final Descriptors.Descriptor wpi_proto_ProtobufDCMotor_descriptor = descriptor.internalContainedType(27, 196, "ProtobufDCMotor", "wpi.proto.ProtobufDCMotor"); - - /** - * @return this proto file's descriptor. - */ - public static Descriptors.FileDescriptor getDescriptor() { - return descriptor; - } - - /** - * Protobuf type {@code ProtobufDCMotor} - */ - public static final class ProtobufDCMotor extends ProtoMessage implements Cloneable { - private static final long serialVersionUID = 0L; - - /** - * optional double nominal_voltage = 1; - */ - private double nominalVoltage; - - /** - * optional double stall_torque = 2; - */ - private double stallTorque; - - /** - * optional double stall_current = 3; - */ - private double stallCurrent; - - /** - * optional double free_current = 4; - */ - private double freeCurrent; - - /** - * optional double free_speed = 5; - */ - private double freeSpeed; - - private ProtobufDCMotor() { - } - - /** - * @return a new empty instance of {@code ProtobufDCMotor} - */ - public static ProtobufDCMotor newInstance() { - return new ProtobufDCMotor(); - } - - /** - * optional double nominal_voltage = 1; - * @return whether the nominalVoltage field is set - */ - public boolean hasNominalVoltage() { - return (bitField0_ & 0x00000001) != 0; - } - - /** - * optional double nominal_voltage = 1; - * @return this - */ - public ProtobufDCMotor clearNominalVoltage() { - bitField0_ &= ~0x00000001; - nominalVoltage = 0D; - return this; - } - - /** - * optional double nominal_voltage = 1; - * @return the nominalVoltage - */ - public double getNominalVoltage() { - return nominalVoltage; - } - - /** - * optional double nominal_voltage = 1; - * @param value the nominalVoltage to set - * @return this - */ - public ProtobufDCMotor setNominalVoltage(final double value) { - bitField0_ |= 0x00000001; - nominalVoltage = value; - return this; - } - - /** - * optional double stall_torque = 2; - * @return whether the stallTorque field is set - */ - public boolean hasStallTorque() { - return (bitField0_ & 0x00000002) != 0; - } - - /** - * optional double stall_torque = 2; - * @return this - */ - public ProtobufDCMotor clearStallTorque() { - bitField0_ &= ~0x00000002; - stallTorque = 0D; - return this; - } - - /** - * optional double stall_torque = 2; - * @return the stallTorque - */ - public double getStallTorque() { - return stallTorque; - } - - /** - * optional double stall_torque = 2; - * @param value the stallTorque to set - * @return this - */ - public ProtobufDCMotor setStallTorque(final double value) { - bitField0_ |= 0x00000002; - stallTorque = value; - return this; - } - - /** - * optional double stall_current = 3; - * @return whether the stallCurrent field is set - */ - public boolean hasStallCurrent() { - return (bitField0_ & 0x00000004) != 0; - } - - /** - * optional double stall_current = 3; - * @return this - */ - public ProtobufDCMotor clearStallCurrent() { - bitField0_ &= ~0x00000004; - stallCurrent = 0D; - return this; - } - - /** - * optional double stall_current = 3; - * @return the stallCurrent - */ - public double getStallCurrent() { - return stallCurrent; - } - - /** - * optional double stall_current = 3; - * @param value the stallCurrent to set - * @return this - */ - public ProtobufDCMotor setStallCurrent(final double value) { - bitField0_ |= 0x00000004; - stallCurrent = value; - return this; - } - - /** - * optional double free_current = 4; - * @return whether the freeCurrent field is set - */ - public boolean hasFreeCurrent() { - return (bitField0_ & 0x00000008) != 0; - } - - /** - * optional double free_current = 4; - * @return this - */ - public ProtobufDCMotor clearFreeCurrent() { - bitField0_ &= ~0x00000008; - freeCurrent = 0D; - return this; - } - - /** - * optional double free_current = 4; - * @return the freeCurrent - */ - public double getFreeCurrent() { - return freeCurrent; - } - - /** - * optional double free_current = 4; - * @param value the freeCurrent to set - * @return this - */ - public ProtobufDCMotor setFreeCurrent(final double value) { - bitField0_ |= 0x00000008; - freeCurrent = value; - return this; - } - - /** - * optional double free_speed = 5; - * @return whether the freeSpeed field is set - */ - public boolean hasFreeSpeed() { - return (bitField0_ & 0x00000010) != 0; - } - - /** - * optional double free_speed = 5; - * @return this - */ - public ProtobufDCMotor clearFreeSpeed() { - bitField0_ &= ~0x00000010; - freeSpeed = 0D; - return this; - } - - /** - * optional double free_speed = 5; - * @return the freeSpeed - */ - public double getFreeSpeed() { - return freeSpeed; - } - - /** - * optional double free_speed = 5; - * @param value the freeSpeed to set - * @return this - */ - public ProtobufDCMotor setFreeSpeed(final double value) { - bitField0_ |= 0x00000010; - freeSpeed = value; - return this; - } - - @Override - public ProtobufDCMotor copyFrom(final ProtobufDCMotor other) { - cachedSize = other.cachedSize; - if ((bitField0_ | other.bitField0_) != 0) { - bitField0_ = other.bitField0_; - nominalVoltage = other.nominalVoltage; - stallTorque = other.stallTorque; - stallCurrent = other.stallCurrent; - freeCurrent = other.freeCurrent; - freeSpeed = other.freeSpeed; - } - return this; - } - - @Override - public ProtobufDCMotor mergeFrom(final ProtobufDCMotor other) { - if (other.isEmpty()) { - return this; - } - cachedSize = -1; - if (other.hasNominalVoltage()) { - setNominalVoltage(other.nominalVoltage); - } - if (other.hasStallTorque()) { - setStallTorque(other.stallTorque); - } - if (other.hasStallCurrent()) { - setStallCurrent(other.stallCurrent); - } - if (other.hasFreeCurrent()) { - setFreeCurrent(other.freeCurrent); - } - if (other.hasFreeSpeed()) { - setFreeSpeed(other.freeSpeed); - } - return this; - } - - @Override - public ProtobufDCMotor clear() { - if (isEmpty()) { - return this; - } - cachedSize = -1; - bitField0_ = 0; - nominalVoltage = 0D; - stallTorque = 0D; - stallCurrent = 0D; - freeCurrent = 0D; - freeSpeed = 0D; - return this; - } - - @Override - public ProtobufDCMotor clearQuick() { - if (isEmpty()) { - return this; - } - cachedSize = -1; - bitField0_ = 0; - return this; - } - - @Override - public boolean equals(Object o) { - if (o == this) { - return true; - } - if (!(o instanceof ProtobufDCMotor)) { - return false; - } - ProtobufDCMotor other = (ProtobufDCMotor) o; - return bitField0_ == other.bitField0_ - && (!hasNominalVoltage() || ProtoUtil.isEqual(nominalVoltage, other.nominalVoltage)) - && (!hasStallTorque() || ProtoUtil.isEqual(stallTorque, other.stallTorque)) - && (!hasStallCurrent() || ProtoUtil.isEqual(stallCurrent, other.stallCurrent)) - && (!hasFreeCurrent() || ProtoUtil.isEqual(freeCurrent, other.freeCurrent)) - && (!hasFreeSpeed() || ProtoUtil.isEqual(freeSpeed, other.freeSpeed)); - } - - @Override - public void writeTo(final ProtoSink output) throws IOException { - if ((bitField0_ & 0x00000001) != 0) { - output.writeRawByte((byte) 9); - output.writeDoubleNoTag(nominalVoltage); - } - if ((bitField0_ & 0x00000002) != 0) { - output.writeRawByte((byte) 17); - output.writeDoubleNoTag(stallTorque); - } - if ((bitField0_ & 0x00000004) != 0) { - output.writeRawByte((byte) 25); - output.writeDoubleNoTag(stallCurrent); - } - if ((bitField0_ & 0x00000008) != 0) { - output.writeRawByte((byte) 33); - output.writeDoubleNoTag(freeCurrent); - } - if ((bitField0_ & 0x00000010) != 0) { - output.writeRawByte((byte) 41); - output.writeDoubleNoTag(freeSpeed); - } - } - - @Override - protected int computeSerializedSize() { - int size = 0; - if ((bitField0_ & 0x00000001) != 0) { - size += 9; - } - if ((bitField0_ & 0x00000002) != 0) { - size += 9; - } - if ((bitField0_ & 0x00000004) != 0) { - size += 9; - } - if ((bitField0_ & 0x00000008) != 0) { - size += 9; - } - if ((bitField0_ & 0x00000010) != 0) { - size += 9; - } - return size; - } - - @Override - @SuppressWarnings("fallthrough") - public ProtobufDCMotor mergeFrom(final ProtoSource input) throws IOException { - // Enabled Fall-Through Optimization (QuickBuffers) - int tag = input.readTag(); - while (true) { - switch (tag) { - case 9: { - // nominalVoltage - nominalVoltage = input.readDouble(); - bitField0_ |= 0x00000001; - tag = input.readTag(); - if (tag != 17) { - break; - } - } - case 17: { - // stallTorque - stallTorque = input.readDouble(); - bitField0_ |= 0x00000002; - tag = input.readTag(); - if (tag != 25) { - break; - } - } - case 25: { - // stallCurrent - stallCurrent = input.readDouble(); - bitField0_ |= 0x00000004; - tag = input.readTag(); - if (tag != 33) { - break; - } - } - case 33: { - // freeCurrent - freeCurrent = input.readDouble(); - bitField0_ |= 0x00000008; - tag = input.readTag(); - if (tag != 41) { - break; - } - } - case 41: { - // freeSpeed - freeSpeed = input.readDouble(); - bitField0_ |= 0x00000010; - tag = input.readTag(); - if (tag != 0) { - break; - } - } - case 0: { - return this; - } - default: { - if (!input.skipField(tag)) { - return this; - } - tag = input.readTag(); - break; - } - } - } - } - - @Override - public void writeTo(final JsonSink output) throws IOException { - output.beginObject(); - if ((bitField0_ & 0x00000001) != 0) { - output.writeDouble(FieldNames.nominalVoltage, nominalVoltage); - } - if ((bitField0_ & 0x00000002) != 0) { - output.writeDouble(FieldNames.stallTorque, stallTorque); - } - if ((bitField0_ & 0x00000004) != 0) { - output.writeDouble(FieldNames.stallCurrent, stallCurrent); - } - if ((bitField0_ & 0x00000008) != 0) { - output.writeDouble(FieldNames.freeCurrent, freeCurrent); - } - if ((bitField0_ & 0x00000010) != 0) { - output.writeDouble(FieldNames.freeSpeed, freeSpeed); - } - output.endObject(); - } - - @Override - public ProtobufDCMotor mergeFrom(final JsonSource input) throws IOException { - if (!input.beginObject()) { - return this; - } - while (!input.isAtEnd()) { - switch (input.readFieldHash()) { - case 1374862050: - case 173092603: { - if (input.isAtField(FieldNames.nominalVoltage)) { - if (!input.trySkipNullValue()) { - nominalVoltage = input.readDouble(); - bitField0_ |= 0x00000001; - } - } else { - input.skipUnknownField(); - } - break; - } - case 2075810250: - case 1238615945: { - if (input.isAtField(FieldNames.stallTorque)) { - if (!input.trySkipNullValue()) { - stallTorque = input.readDouble(); - bitField0_ |= 0x00000002; - } - } else { - input.skipUnknownField(); - } - break; - } - case -2105262663: - case 2006484954: { - if (input.isAtField(FieldNames.stallCurrent)) { - if (!input.trySkipNullValue()) { - stallCurrent = input.readDouble(); - bitField0_ |= 0x00000004; - } - } else { - input.skipUnknownField(); - } - break; - } - case 1024355693: - case 240406182: { - if (input.isAtField(FieldNames.freeCurrent)) { - if (!input.trySkipNullValue()) { - freeCurrent = input.readDouble(); - bitField0_ |= 0x00000008; - } - } else { - input.skipUnknownField(); - } - break; - } - case -444654277: - case -552732492: { - if (input.isAtField(FieldNames.freeSpeed)) { - if (!input.trySkipNullValue()) { - freeSpeed = input.readDouble(); - bitField0_ |= 0x00000010; - } - } else { - input.skipUnknownField(); - } - break; - } - default: { - input.skipUnknownField(); - break; - } - } - } - input.endObject(); - return this; - } - - @Override - public ProtobufDCMotor clone() { - return new ProtobufDCMotor().copyFrom(this); - } - - @Override - public boolean isEmpty() { - return ((bitField0_) == 0); - } - - public static ProtobufDCMotor parseFrom(final byte[] data) throws - InvalidProtocolBufferException { - return ProtoMessage.mergeFrom(new ProtobufDCMotor(), data).checkInitialized(); - } - - public static ProtobufDCMotor parseFrom(final ProtoSource input) throws IOException { - return ProtoMessage.mergeFrom(new ProtobufDCMotor(), input).checkInitialized(); - } - - public static ProtobufDCMotor parseFrom(final JsonSource input) throws IOException { - return ProtoMessage.mergeFrom(new ProtobufDCMotor(), input).checkInitialized(); - } - - /** - * @return factory for creating ProtobufDCMotor messages - */ - public static MessageFactory getFactory() { - return ProtobufDCMotorFactory.INSTANCE; - } - - /** - * @return this type's descriptor. - */ - public static Descriptors.Descriptor getDescriptor() { - return Plant.wpi_proto_ProtobufDCMotor_descriptor; - } - - private enum ProtobufDCMotorFactory implements MessageFactory { - INSTANCE; - - @Override - public ProtobufDCMotor create() { - return ProtobufDCMotor.newInstance(); - } - } - - /** - * Contains name constants used for serializing JSON - */ - static class FieldNames { - static final FieldName nominalVoltage = FieldName.forField("nominalVoltage", "nominal_voltage"); - - static final FieldName stallTorque = FieldName.forField("stallTorque", "stall_torque"); - - static final FieldName stallCurrent = FieldName.forField("stallCurrent", "stall_current"); - - static final FieldName freeCurrent = FieldName.forField("freeCurrent", "free_current"); - - static final FieldName freeSpeed = FieldName.forField("freeSpeed", "free_speed"); - } - } -} diff --git a/wpimath/src/generated/main/java/org/wpilib/math/proto/System.java b/wpimath/src/generated/main/java/org/wpilib/math/proto/System.java index 7d9a2182da..06a2df7a40 100644 --- a/wpimath/src/generated/main/java/org/wpilib/math/proto/System.java +++ b/wpimath/src/generated/main/java/org/wpilib/math/proto/System.java @@ -18,25 +18,36 @@ import us.hebi.quickbuf.ProtoUtil; import us.hebi.quickbuf.RepeatedByte; public final class System { - private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(829, - "CgxzeXN0ZW0ucHJvdG8SCXdwaS5wcm90bxoNd3BpbWF0aC5wcm90byKZAgoUUHJvdG9idWZMaW5lYXJT" + - "eXN0ZW0SHQoKbnVtX3N0YXRlcxgBIAEoDVIJbnVtU3RhdGVzEh0KCm51bV9pbnB1dHMYAiABKA1SCW51" + - "bUlucHV0cxIfCgtudW1fb3V0cHV0cxgDIAEoDVIKbnVtT3V0cHV0cxInCgFhGAQgASgLMhkud3BpLnBy" + - "b3RvLlByb3RvYnVmTWF0cml4UgFhEicKAWIYBSABKAsyGS53cGkucHJvdG8uUHJvdG9idWZNYXRyaXhS" + - "AWISJwoBYxgGIAEoCzIZLndwaS5wcm90by5Qcm90b2J1Zk1hdHJpeFIBYxInCgFkGAcgASgLMhkud3Bp" + - "LnByb3RvLlByb3RvYnVmTWF0cml4UgFkQhcKFW9yZy53cGlsaWIubWF0aC5wcm90b0rVAwoGEgQAABAB" + - "CggKAQwSAwAAEgoICgECEgMCABIKCQoCAwASAwQAFwoICgEIEgMGAC4KCQoCCAESAwYALgoKCgIEABIE" + - "CAAQAQoKCgMEAAESAwgIHAoLCgQEAAIAEgMJAhgKDAoFBAACAAUSAwkCCAoMCgUEAAIAARIDCQkTCgwK" + - "BQQAAgADEgMJFhcKCwoEBAACARIDCgIYCgwKBQQAAgEFEgMKAggKDAoFBAACAQESAwoJEwoMCgUEAAIB" + - "AxIDChYXCgsKBAQAAgISAwsCGQoMCgUEAAICBRIDCwIICgwKBQQAAgIBEgMLCRQKDAoFBAACAgMSAwsX" + - "GAoLCgQEAAIDEgMMAhcKDAoFBAACAwYSAwwCEAoMCgUEAAIDARIDDBESCgwKBQQAAgMDEgMMFRYKCwoE" + - "BAACBBIDDQIXCgwKBQQAAgQGEgMNAhAKDAoFBAACBAESAw0REgoMCgUEAAIEAxIDDRUWCgsKBAQAAgUS" + - "Aw4CFwoMCgUEAAIFBhIDDgIQCgwKBQQAAgUBEgMOERIKDAoFBAACBQMSAw4VFgoLCgQEAAIGEgMPAhcK" + - "DAoFBAACBgYSAw8CEAoMCgUEAAIGARIDDxESCgwKBQQAAgYDEgMPFRZiBnByb3RvMw=="); + private static final RepeatedByte descriptorData = ProtoUtil.decodeBase64(1327, + "CgxzeXN0ZW0ucHJvdG8SCXdwaS5wcm90bxoNd3BpbWF0aC5wcm90byLEAQoPUHJvdG9idWZEQ01vdG9y" + + "EicKD25vbWluYWxfdm9sdGFnZRgBIAEoAVIObm9taW5hbFZvbHRhZ2USIQoMc3RhbGxfdG9ycXVlGAIg" + + "ASgBUgtzdGFsbFRvcnF1ZRIjCg1zdGFsbF9jdXJyZW50GAMgASgBUgxzdGFsbEN1cnJlbnQSIQoMZnJl" + + "ZV9jdXJyZW50GAQgASgBUgtmcmVlQ3VycmVudBIdCgpmcmVlX3NwZWVkGAUgASgBUglmcmVlU3BlZWQi" + + "mQIKFFByb3RvYnVmTGluZWFyU3lzdGVtEh0KCm51bV9zdGF0ZXMYASABKA1SCW51bVN0YXRlcxIdCgpu" + + "dW1faW5wdXRzGAIgASgNUgludW1JbnB1dHMSHwoLbnVtX291dHB1dHMYAyABKA1SCm51bU91dHB1dHMS" + + "JwoBYRgEIAEoCzIZLndwaS5wcm90by5Qcm90b2J1Zk1hdHJpeFIBYRInCgFiGAUgASgLMhkud3BpLnBy" + + "b3RvLlByb3RvYnVmTWF0cml4UgFiEicKAWMYBiABKAsyGS53cGkucHJvdG8uUHJvdG9idWZNYXRyaXhS" + + "AWMSJwoBZBgHIAEoCzIZLndwaS5wcm90by5Qcm90b2J1Zk1hdHJpeFIBZEIXChVvcmcud3BpbGliLm1h" + + "dGgucHJvdG9KgAYKBhIEAAAYAQoICgEMEgMAABIKCAoBAhIDAgASCgkKAgMAEgMEABcKCAoBCBIDBgAu" + + "CgkKAggBEgMGAC4KCgoCBAASBAgADgEKCgoDBAABEgMICBcKCwoEBAACABIDCQIdCgwKBQQAAgAFEgMJ" + + "AggKDAoFBAACAAESAwkJGAoMCgUEAAIAAxIDCRscCgsKBAQAAgESAwoCGgoMCgUEAAIBBRIDCgIICgwK" + + "BQQAAgEBEgMKCRUKDAoFBAACAQMSAwoYGQoLCgQEAAICEgMLAhsKDAoFBAACAgUSAwsCCAoMCgUEAAIC" + + "ARIDCwkWCgwKBQQAAgIDEgMLGRoKCwoEBAACAxIDDAIaCgwKBQQAAgMFEgMMAggKDAoFBAACAwESAwwJ" + + "FQoMCgUEAAIDAxIDDBgZCgsKBAQAAgQSAw0CGAoMCgUEAAIEBRIDDQIICgwKBQQAAgQBEgMNCRMKDAoF" + + "BAACBAMSAw0WFwoKCgIEARIEEAAYAQoKCgMEAQESAxAIHAoLCgQEAQIAEgMRAhgKDAoFBAECAAUSAxEC" + + "CAoMCgUEAQIAARIDEQkTCgwKBQQBAgADEgMRFhcKCwoEBAECARIDEgIYCgwKBQQBAgEFEgMSAggKDAoF" + + "BAECAQESAxIJEwoMCgUEAQIBAxIDEhYXCgsKBAQBAgISAxMCGQoMCgUEAQICBRIDEwIICgwKBQQBAgIB" + + "EgMTCRQKDAoFBAECAgMSAxMXGAoLCgQEAQIDEgMUAhcKDAoFBAECAwYSAxQCEAoMCgUEAQIDARIDFBES" + + "CgwKBQQBAgMDEgMUFRYKCwoEBAECBBIDFQIXCgwKBQQBAgQGEgMVAhAKDAoFBAECBAESAxUREgoMCgUE", + "AQIEAxIDFRUWCgsKBAQBAgUSAxYCFwoMCgUEAQIFBhIDFgIQCgwKBQQBAgUBEgMWERIKDAoFBAECBQMS" + + "AxYVFgoLCgQEAQIGEgMXAhcKDAoFBAECBgYSAxcCEAoMCgUEAQIGARIDFxESCgwKBQQBAgYDEgMXFRZi" + + "BnByb3RvMw=="); static final Descriptors.FileDescriptor descriptor = Descriptors.FileDescriptor.internalBuildGeneratedFileFrom("system.proto", "wpi.proto", descriptorData, Wpimath.getDescriptor()); - static final Descriptors.Descriptor wpi_proto_ProtobufLinearSystem_descriptor = descriptor.internalContainedType(43, 281, "ProtobufLinearSystem", "wpi.proto.ProtobufLinearSystem"); + static final Descriptors.Descriptor wpi_proto_ProtobufDCMotor_descriptor = descriptor.internalContainedType(43, 196, "ProtobufDCMotor", "wpi.proto.ProtobufDCMotor"); + + static final Descriptors.Descriptor wpi_proto_ProtobufLinearSystem_descriptor = descriptor.internalContainedType(242, 281, "ProtobufLinearSystem", "wpi.proto.ProtobufLinearSystem"); /** * @return this proto file's descriptor. @@ -45,6 +56,583 @@ public final class System { return descriptor; } + /** + * Protobuf type {@code ProtobufDCMotor} + */ + public static final class ProtobufDCMotor extends ProtoMessage implements Cloneable { + private static final long serialVersionUID = 0L; + + /** + * optional double nominal_voltage = 1; + */ + private double nominalVoltage; + + /** + * optional double stall_torque = 2; + */ + private double stallTorque; + + /** + * optional double stall_current = 3; + */ + private double stallCurrent; + + /** + * optional double free_current = 4; + */ + private double freeCurrent; + + /** + * optional double free_speed = 5; + */ + private double freeSpeed; + + private ProtobufDCMotor() { + } + + /** + * @return a new empty instance of {@code ProtobufDCMotor} + */ + public static ProtobufDCMotor newInstance() { + return new ProtobufDCMotor(); + } + + /** + * optional double nominal_voltage = 1; + * @return whether the nominalVoltage field is set + */ + public boolean hasNominalVoltage() { + return (bitField0_ & 0x00000001) != 0; + } + + /** + * optional double nominal_voltage = 1; + * @return this + */ + public ProtobufDCMotor clearNominalVoltage() { + bitField0_ &= ~0x00000001; + nominalVoltage = 0D; + return this; + } + + /** + * optional double nominal_voltage = 1; + * @return the nominalVoltage + */ + public double getNominalVoltage() { + return nominalVoltage; + } + + /** + * optional double nominal_voltage = 1; + * @param value the nominalVoltage to set + * @return this + */ + public ProtobufDCMotor setNominalVoltage(final double value) { + bitField0_ |= 0x00000001; + nominalVoltage = value; + return this; + } + + /** + * optional double stall_torque = 2; + * @return whether the stallTorque field is set + */ + public boolean hasStallTorque() { + return (bitField0_ & 0x00000002) != 0; + } + + /** + * optional double stall_torque = 2; + * @return this + */ + public ProtobufDCMotor clearStallTorque() { + bitField0_ &= ~0x00000002; + stallTorque = 0D; + return this; + } + + /** + * optional double stall_torque = 2; + * @return the stallTorque + */ + public double getStallTorque() { + return stallTorque; + } + + /** + * optional double stall_torque = 2; + * @param value the stallTorque to set + * @return this + */ + public ProtobufDCMotor setStallTorque(final double value) { + bitField0_ |= 0x00000002; + stallTorque = value; + return this; + } + + /** + * optional double stall_current = 3; + * @return whether the stallCurrent field is set + */ + public boolean hasStallCurrent() { + return (bitField0_ & 0x00000004) != 0; + } + + /** + * optional double stall_current = 3; + * @return this + */ + public ProtobufDCMotor clearStallCurrent() { + bitField0_ &= ~0x00000004; + stallCurrent = 0D; + return this; + } + + /** + * optional double stall_current = 3; + * @return the stallCurrent + */ + public double getStallCurrent() { + return stallCurrent; + } + + /** + * optional double stall_current = 3; + * @param value the stallCurrent to set + * @return this + */ + public ProtobufDCMotor setStallCurrent(final double value) { + bitField0_ |= 0x00000004; + stallCurrent = value; + return this; + } + + /** + * optional double free_current = 4; + * @return whether the freeCurrent field is set + */ + public boolean hasFreeCurrent() { + return (bitField0_ & 0x00000008) != 0; + } + + /** + * optional double free_current = 4; + * @return this + */ + public ProtobufDCMotor clearFreeCurrent() { + bitField0_ &= ~0x00000008; + freeCurrent = 0D; + return this; + } + + /** + * optional double free_current = 4; + * @return the freeCurrent + */ + public double getFreeCurrent() { + return freeCurrent; + } + + /** + * optional double free_current = 4; + * @param value the freeCurrent to set + * @return this + */ + public ProtobufDCMotor setFreeCurrent(final double value) { + bitField0_ |= 0x00000008; + freeCurrent = value; + return this; + } + + /** + * optional double free_speed = 5; + * @return whether the freeSpeed field is set + */ + public boolean hasFreeSpeed() { + return (bitField0_ & 0x00000010) != 0; + } + + /** + * optional double free_speed = 5; + * @return this + */ + public ProtobufDCMotor clearFreeSpeed() { + bitField0_ &= ~0x00000010; + freeSpeed = 0D; + return this; + } + + /** + * optional double free_speed = 5; + * @return the freeSpeed + */ + public double getFreeSpeed() { + return freeSpeed; + } + + /** + * optional double free_speed = 5; + * @param value the freeSpeed to set + * @return this + */ + public ProtobufDCMotor setFreeSpeed(final double value) { + bitField0_ |= 0x00000010; + freeSpeed = value; + return this; + } + + @Override + public ProtobufDCMotor copyFrom(final ProtobufDCMotor other) { + cachedSize = other.cachedSize; + if ((bitField0_ | other.bitField0_) != 0) { + bitField0_ = other.bitField0_; + nominalVoltage = other.nominalVoltage; + stallTorque = other.stallTorque; + stallCurrent = other.stallCurrent; + freeCurrent = other.freeCurrent; + freeSpeed = other.freeSpeed; + } + return this; + } + + @Override + public ProtobufDCMotor mergeFrom(final ProtobufDCMotor other) { + if (other.isEmpty()) { + return this; + } + cachedSize = -1; + if (other.hasNominalVoltage()) { + setNominalVoltage(other.nominalVoltage); + } + if (other.hasStallTorque()) { + setStallTorque(other.stallTorque); + } + if (other.hasStallCurrent()) { + setStallCurrent(other.stallCurrent); + } + if (other.hasFreeCurrent()) { + setFreeCurrent(other.freeCurrent); + } + if (other.hasFreeSpeed()) { + setFreeSpeed(other.freeSpeed); + } + return this; + } + + @Override + public ProtobufDCMotor clear() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + nominalVoltage = 0D; + stallTorque = 0D; + stallCurrent = 0D; + freeCurrent = 0D; + freeSpeed = 0D; + return this; + } + + @Override + public ProtobufDCMotor clearQuick() { + if (isEmpty()) { + return this; + } + cachedSize = -1; + bitField0_ = 0; + return this; + } + + @Override + public boolean equals(Object o) { + if (o == this) { + return true; + } + if (!(o instanceof ProtobufDCMotor)) { + return false; + } + ProtobufDCMotor other = (ProtobufDCMotor) o; + return bitField0_ == other.bitField0_ + && (!hasNominalVoltage() || ProtoUtil.isEqual(nominalVoltage, other.nominalVoltage)) + && (!hasStallTorque() || ProtoUtil.isEqual(stallTorque, other.stallTorque)) + && (!hasStallCurrent() || ProtoUtil.isEqual(stallCurrent, other.stallCurrent)) + && (!hasFreeCurrent() || ProtoUtil.isEqual(freeCurrent, other.freeCurrent)) + && (!hasFreeSpeed() || ProtoUtil.isEqual(freeSpeed, other.freeSpeed)); + } + + @Override + public void writeTo(final ProtoSink output) throws IOException { + if ((bitField0_ & 0x00000001) != 0) { + output.writeRawByte((byte) 9); + output.writeDoubleNoTag(nominalVoltage); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeRawByte((byte) 17); + output.writeDoubleNoTag(stallTorque); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeRawByte((byte) 25); + output.writeDoubleNoTag(stallCurrent); + } + if ((bitField0_ & 0x00000008) != 0) { + output.writeRawByte((byte) 33); + output.writeDoubleNoTag(freeCurrent); + } + if ((bitField0_ & 0x00000010) != 0) { + output.writeRawByte((byte) 41); + output.writeDoubleNoTag(freeSpeed); + } + } + + @Override + protected int computeSerializedSize() { + int size = 0; + if ((bitField0_ & 0x00000001) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000002) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000004) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000008) != 0) { + size += 9; + } + if ((bitField0_ & 0x00000010) != 0) { + size += 9; + } + return size; + } + + @Override + @SuppressWarnings("fallthrough") + public ProtobufDCMotor mergeFrom(final ProtoSource input) throws IOException { + // Enabled Fall-Through Optimization (QuickBuffers) + int tag = input.readTag(); + while (true) { + switch (tag) { + case 9: { + // nominalVoltage + nominalVoltage = input.readDouble(); + bitField0_ |= 0x00000001; + tag = input.readTag(); + if (tag != 17) { + break; + } + } + case 17: { + // stallTorque + stallTorque = input.readDouble(); + bitField0_ |= 0x00000002; + tag = input.readTag(); + if (tag != 25) { + break; + } + } + case 25: { + // stallCurrent + stallCurrent = input.readDouble(); + bitField0_ |= 0x00000004; + tag = input.readTag(); + if (tag != 33) { + break; + } + } + case 33: { + // freeCurrent + freeCurrent = input.readDouble(); + bitField0_ |= 0x00000008; + tag = input.readTag(); + if (tag != 41) { + break; + } + } + case 41: { + // freeSpeed + freeSpeed = input.readDouble(); + bitField0_ |= 0x00000010; + tag = input.readTag(); + if (tag != 0) { + break; + } + } + case 0: { + return this; + } + default: { + if (!input.skipField(tag)) { + return this; + } + tag = input.readTag(); + break; + } + } + } + } + + @Override + public void writeTo(final JsonSink output) throws IOException { + output.beginObject(); + if ((bitField0_ & 0x00000001) != 0) { + output.writeDouble(FieldNames.nominalVoltage, nominalVoltage); + } + if ((bitField0_ & 0x00000002) != 0) { + output.writeDouble(FieldNames.stallTorque, stallTorque); + } + if ((bitField0_ & 0x00000004) != 0) { + output.writeDouble(FieldNames.stallCurrent, stallCurrent); + } + if ((bitField0_ & 0x00000008) != 0) { + output.writeDouble(FieldNames.freeCurrent, freeCurrent); + } + if ((bitField0_ & 0x00000010) != 0) { + output.writeDouble(FieldNames.freeSpeed, freeSpeed); + } + output.endObject(); + } + + @Override + public ProtobufDCMotor mergeFrom(final JsonSource input) throws IOException { + if (!input.beginObject()) { + return this; + } + while (!input.isAtEnd()) { + switch (input.readFieldHash()) { + case 1374862050: + case 173092603: { + if (input.isAtField(FieldNames.nominalVoltage)) { + if (!input.trySkipNullValue()) { + nominalVoltage = input.readDouble(); + bitField0_ |= 0x00000001; + } + } else { + input.skipUnknownField(); + } + break; + } + case 2075810250: + case 1238615945: { + if (input.isAtField(FieldNames.stallTorque)) { + if (!input.trySkipNullValue()) { + stallTorque = input.readDouble(); + bitField0_ |= 0x00000002; + } + } else { + input.skipUnknownField(); + } + break; + } + case -2105262663: + case 2006484954: { + if (input.isAtField(FieldNames.stallCurrent)) { + if (!input.trySkipNullValue()) { + stallCurrent = input.readDouble(); + bitField0_ |= 0x00000004; + } + } else { + input.skipUnknownField(); + } + break; + } + case 1024355693: + case 240406182: { + if (input.isAtField(FieldNames.freeCurrent)) { + if (!input.trySkipNullValue()) { + freeCurrent = input.readDouble(); + bitField0_ |= 0x00000008; + } + } else { + input.skipUnknownField(); + } + break; + } + case -444654277: + case -552732492: { + if (input.isAtField(FieldNames.freeSpeed)) { + if (!input.trySkipNullValue()) { + freeSpeed = input.readDouble(); + bitField0_ |= 0x00000010; + } + } else { + input.skipUnknownField(); + } + break; + } + default: { + input.skipUnknownField(); + break; + } + } + } + input.endObject(); + return this; + } + + @Override + public ProtobufDCMotor clone() { + return new ProtobufDCMotor().copyFrom(this); + } + + @Override + public boolean isEmpty() { + return ((bitField0_) == 0); + } + + public static ProtobufDCMotor parseFrom(final byte[] data) throws + InvalidProtocolBufferException { + return ProtoMessage.mergeFrom(new ProtobufDCMotor(), data).checkInitialized(); + } + + public static ProtobufDCMotor parseFrom(final ProtoSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufDCMotor(), input).checkInitialized(); + } + + public static ProtobufDCMotor parseFrom(final JsonSource input) throws IOException { + return ProtoMessage.mergeFrom(new ProtobufDCMotor(), input).checkInitialized(); + } + + /** + * @return factory for creating ProtobufDCMotor messages + */ + public static MessageFactory getFactory() { + return ProtobufDCMotorFactory.INSTANCE; + } + + /** + * @return this type's descriptor. + */ + public static Descriptors.Descriptor getDescriptor() { + return System.wpi_proto_ProtobufDCMotor_descriptor; + } + + private enum ProtobufDCMotorFactory implements MessageFactory { + INSTANCE; + + @Override + public ProtobufDCMotor create() { + return ProtobufDCMotor.newInstance(); + } + } + + /** + * Contains name constants used for serializing JSON + */ + static class FieldNames { + static final FieldName nominalVoltage = FieldName.forField("nominalVoltage", "nominal_voltage"); + + static final FieldName stallTorque = FieldName.forField("stallTorque", "stall_torque"); + + static final FieldName stallCurrent = FieldName.forField("stallCurrent", "stall_current"); + + static final FieldName freeCurrent = FieldName.forField("freeCurrent", "free_current"); + + static final FieldName freeSpeed = FieldName.forField("freeSpeed", "free_speed"); + } + } + /** * Protobuf type {@code ProtobufLinearSystem} */ diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/plant.npb.cpp b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/plant.npb.cpp deleted file mode 100644 index d5c2b30824..0000000000 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/plant.npb.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// 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. -/* Automatically generated nanopb constant definitions */ -/* Generated by nanopb-0.4.9 */ - -#include "plant.npb.h" -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -#include -#include -static const uint8_t file_descriptor[] { -0x0a,0x0b,0x70,0x6c,0x61,0x6e,0x74,0x2e,0x70,0x72, -0x6f,0x74,0x6f,0x12,0x09,0x77,0x70,0x69,0x2e,0x70, -0x72,0x6f,0x74,0x6f,0x22,0xc4,0x01,0x0a,0x0f,0x50, -0x72,0x6f,0x74,0x6f,0x62,0x75,0x66,0x44,0x43,0x4d, -0x6f,0x74,0x6f,0x72,0x12,0x27,0x0a,0x0f,0x6e,0x6f, -0x6d,0x69,0x6e,0x61,0x6c,0x5f,0x76,0x6f,0x6c,0x74, -0x61,0x67,0x65,0x18,0x01,0x20,0x01,0x28,0x01,0x52, -0x0e,0x6e,0x6f,0x6d,0x69,0x6e,0x61,0x6c,0x56,0x6f, -0x6c,0x74,0x61,0x67,0x65,0x12,0x21,0x0a,0x0c,0x73, -0x74,0x61,0x6c,0x6c,0x5f,0x74,0x6f,0x72,0x71,0x75, -0x65,0x18,0x02,0x20,0x01,0x28,0x01,0x52,0x0b,0x73, -0x74,0x61,0x6c,0x6c,0x54,0x6f,0x72,0x71,0x75,0x65, -0x12,0x23,0x0a,0x0d,0x73,0x74,0x61,0x6c,0x6c,0x5f, -0x63,0x75,0x72,0x72,0x65,0x6e,0x74,0x18,0x03,0x20, -0x01,0x28,0x01,0x52,0x0c,0x73,0x74,0x61,0x6c,0x6c, -0x43,0x75,0x72,0x72,0x65,0x6e,0x74,0x12,0x21,0x0a, -0x0c,0x66,0x72,0x65,0x65,0x5f,0x63,0x75,0x72,0x72, -0x65,0x6e,0x74,0x18,0x04,0x20,0x01,0x28,0x01,0x52, -0x0b,0x66,0x72,0x65,0x65,0x43,0x75,0x72,0x72,0x65, -0x6e,0x74,0x12,0x1d,0x0a,0x0a,0x66,0x72,0x65,0x65, -0x5f,0x73,0x70,0x65,0x65,0x64,0x18,0x05,0x20,0x01, -0x28,0x01,0x52,0x09,0x66,0x72,0x65,0x65,0x53,0x70, -0x65,0x65,0x64,0x42,0x17,0x0a,0x15,0x6f,0x72,0x67, -0x2e,0x77,0x70,0x69,0x6c,0x69,0x62,0x2e,0x6d,0x61, -0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0xdc, -0x02,0x0a,0x06,0x12,0x04,0x00,0x00,0x0c,0x01,0x0a, -0x08,0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a, -0x08,0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a, -0x08,0x0a,0x01,0x08,0x12,0x03,0x04,0x00,0x2e,0x0a, -0x09,0x0a,0x02,0x08,0x01,0x12,0x03,0x04,0x00,0x2e, -0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x06,0x00, -0x0c,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01,0x12, -0x03,0x06,0x08,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x00, -0x02,0x00,0x12,0x03,0x07,0x02,0x1d,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x07,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x01, -0x12,0x03,0x07,0x09,0x18,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x00,0x03,0x12,0x03,0x07,0x1b,0x1c,0x0a, -0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03,0x08, -0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01, -0x05,0x12,0x03,0x08,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x08,0x09,0x15, -0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03,0x12, -0x03,0x08,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x00, -0x02,0x02,0x12,0x03,0x09,0x02,0x1b,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x09,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x01, -0x12,0x03,0x09,0x09,0x16,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x02,0x03,0x12,0x03,0x09,0x19,0x1a,0x0a, -0x0b,0x0a,0x04,0x04,0x00,0x02,0x03,0x12,0x03,0x0a, -0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x03, -0x05,0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a,0x05, -0x04,0x00,0x02,0x03,0x01,0x12,0x03,0x0a,0x09,0x15, -0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x03,0x03,0x12, -0x03,0x0a,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04,0x00, -0x02,0x04,0x12,0x03,0x0b,0x02,0x18,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x04,0x05,0x12,0x03,0x0b,0x02, -0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x04,0x01, -0x12,0x03,0x0b,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04, -0x00,0x02,0x04,0x03,0x12,0x03,0x0b,0x16,0x17,0x62, -0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, -}; -static const char file_name[] = "plant.proto"; -static const char wpi_proto_ProtobufDCMotor_name[] = "wpi.proto.ProtobufDCMotor"; -std::string_view wpi_proto_ProtobufDCMotor::msg_name(void) noexcept { return wpi_proto_ProtobufDCMotor_name; } -pb_filedesc_t wpi_proto_ProtobufDCMotor::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } -PB_BIND(wpi_proto_ProtobufDCMotor, wpi_proto_ProtobufDCMotor, AUTO) - - - -#ifndef PB_CONVERT_DOUBLE_FLOAT -/* On some platforms (such as AVR), double is really float. - * To be able to encode/decode double on these platforms, you need. - * to define PB_CONVERT_DOUBLE_FLOAT in pb.h or compiler command line. - */ -PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) -#endif - diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/plant.npb.h b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/plant.npb.h deleted file mode 100644 index 47e14486ae..0000000000 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/plant.npb.h +++ /dev/null @@ -1,57 +0,0 @@ -// 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. -/* Automatically generated nanopb header */ -/* Generated by nanopb-0.4.9 */ - -#ifndef PB_WPI_PROTO_PLANT_NPB_H_INCLUDED -#define PB_WPI_PROTO_PLANT_NPB_H_INCLUDED -#include -#include -#include - -#if PB_PROTO_HEADER_VERSION != 40 -#error Regenerate this file with the current version of nanopb generator. -#endif - -/* Struct definitions */ -typedef struct _wpi_proto_ProtobufDCMotor { - static const pb_msgdesc_t* msg_descriptor(void) noexcept; - static std::string_view msg_name(void) noexcept; - static pb_filedesc_t file_descriptor(void) noexcept; - - double nominal_voltage; - double stall_torque; - double stall_current; - double free_current; - double free_speed; -} wpi_proto_ProtobufDCMotor; - - -/* Initializer values for message structs */ -#define wpi_proto_ProtobufDCMotor_init_default {0, 0, 0, 0, 0} -#define wpi_proto_ProtobufDCMotor_init_zero {0, 0, 0, 0, 0} - -/* Field tags (for use in manual encoding/decoding) */ -#define wpi_proto_ProtobufDCMotor_nominal_voltage_tag 1 -#define wpi_proto_ProtobufDCMotor_stall_torque_tag 2 -#define wpi_proto_ProtobufDCMotor_stall_current_tag 3 -#define wpi_proto_ProtobufDCMotor_free_current_tag 4 -#define wpi_proto_ProtobufDCMotor_free_speed_tag 5 - -/* Struct field encoding specification for nanopb */ -#define wpi_proto_ProtobufDCMotor_FIELDLIST(X, a) \ -X(a, STATIC, SINGULAR, DOUBLE, nominal_voltage, 1) \ -X(a, STATIC, SINGULAR, DOUBLE, stall_torque, 2) \ -X(a, STATIC, SINGULAR, DOUBLE, stall_current, 3) \ -X(a, STATIC, SINGULAR, DOUBLE, free_current, 4) \ -X(a, STATIC, SINGULAR, DOUBLE, free_speed, 5) -#define wpi_proto_ProtobufDCMotor_CALLBACK NULL -#define wpi_proto_ProtobufDCMotor_DEFAULT NULL - -/* Maximum encoded size of messages (where known) */ -#define WPI_PROTO_PLANT_NPB_H_MAX_SIZE wpi_proto_ProtobufDCMotor_size -#define wpi_proto_ProtobufDCMotor_size 45 - - -#endif diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/system.npb.cpp b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/system.npb.cpp index c2444971e8..f9e8537f9e 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/system.npb.cpp +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/system.npb.cpp @@ -16,87 +16,143 @@ static const uint8_t file_descriptor[] { 0x72,0x6f,0x74,0x6f,0x12,0x09,0x77,0x70,0x69,0x2e, 0x70,0x72,0x6f,0x74,0x6f,0x1a,0x0d,0x77,0x70,0x69, 0x6d,0x61,0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f, -0x22,0x99,0x02,0x0a,0x14,0x50,0x72,0x6f,0x74,0x6f, -0x62,0x75,0x66,0x4c,0x69,0x6e,0x65,0x61,0x72,0x53, -0x79,0x73,0x74,0x65,0x6d,0x12,0x1d,0x0a,0x0a,0x6e, -0x75,0x6d,0x5f,0x73,0x74,0x61,0x74,0x65,0x73,0x18, -0x01,0x20,0x01,0x28,0x0d,0x52,0x09,0x6e,0x75,0x6d, -0x53,0x74,0x61,0x74,0x65,0x73,0x12,0x1d,0x0a,0x0a, -0x6e,0x75,0x6d,0x5f,0x69,0x6e,0x70,0x75,0x74,0x73, -0x18,0x02,0x20,0x01,0x28,0x0d,0x52,0x09,0x6e,0x75, -0x6d,0x49,0x6e,0x70,0x75,0x74,0x73,0x12,0x1f,0x0a, -0x0b,0x6e,0x75,0x6d,0x5f,0x6f,0x75,0x74,0x70,0x75, -0x74,0x73,0x18,0x03,0x20,0x01,0x28,0x0d,0x52,0x0a, -0x6e,0x75,0x6d,0x4f,0x75,0x74,0x70,0x75,0x74,0x73, -0x12,0x27,0x0a,0x01,0x61,0x18,0x04,0x20,0x01,0x28, +0x22,0xc4,0x01,0x0a,0x0f,0x50,0x72,0x6f,0x74,0x6f, +0x62,0x75,0x66,0x44,0x43,0x4d,0x6f,0x74,0x6f,0x72, +0x12,0x27,0x0a,0x0f,0x6e,0x6f,0x6d,0x69,0x6e,0x61, +0x6c,0x5f,0x76,0x6f,0x6c,0x74,0x61,0x67,0x65,0x18, +0x01,0x20,0x01,0x28,0x01,0x52,0x0e,0x6e,0x6f,0x6d, +0x69,0x6e,0x61,0x6c,0x56,0x6f,0x6c,0x74,0x61,0x67, +0x65,0x12,0x21,0x0a,0x0c,0x73,0x74,0x61,0x6c,0x6c, +0x5f,0x74,0x6f,0x72,0x71,0x75,0x65,0x18,0x02,0x20, +0x01,0x28,0x01,0x52,0x0b,0x73,0x74,0x61,0x6c,0x6c, +0x54,0x6f,0x72,0x71,0x75,0x65,0x12,0x23,0x0a,0x0d, +0x73,0x74,0x61,0x6c,0x6c,0x5f,0x63,0x75,0x72,0x72, +0x65,0x6e,0x74,0x18,0x03,0x20,0x01,0x28,0x01,0x52, +0x0c,0x73,0x74,0x61,0x6c,0x6c,0x43,0x75,0x72,0x72, +0x65,0x6e,0x74,0x12,0x21,0x0a,0x0c,0x66,0x72,0x65, +0x65,0x5f,0x63,0x75,0x72,0x72,0x65,0x6e,0x74,0x18, +0x04,0x20,0x01,0x28,0x01,0x52,0x0b,0x66,0x72,0x65, +0x65,0x43,0x75,0x72,0x72,0x65,0x6e,0x74,0x12,0x1d, +0x0a,0x0a,0x66,0x72,0x65,0x65,0x5f,0x73,0x70,0x65, +0x65,0x64,0x18,0x05,0x20,0x01,0x28,0x01,0x52,0x09, +0x66,0x72,0x65,0x65,0x53,0x70,0x65,0x65,0x64,0x22, +0x99,0x02,0x0a,0x14,0x50,0x72,0x6f,0x74,0x6f,0x62, +0x75,0x66,0x4c,0x69,0x6e,0x65,0x61,0x72,0x53,0x79, +0x73,0x74,0x65,0x6d,0x12,0x1d,0x0a,0x0a,0x6e,0x75, +0x6d,0x5f,0x73,0x74,0x61,0x74,0x65,0x73,0x18,0x01, +0x20,0x01,0x28,0x0d,0x52,0x09,0x6e,0x75,0x6d,0x53, +0x74,0x61,0x74,0x65,0x73,0x12,0x1d,0x0a,0x0a,0x6e, +0x75,0x6d,0x5f,0x69,0x6e,0x70,0x75,0x74,0x73,0x18, +0x02,0x20,0x01,0x28,0x0d,0x52,0x09,0x6e,0x75,0x6d, +0x49,0x6e,0x70,0x75,0x74,0x73,0x12,0x1f,0x0a,0x0b, +0x6e,0x75,0x6d,0x5f,0x6f,0x75,0x74,0x70,0x75,0x74, +0x73,0x18,0x03,0x20,0x01,0x28,0x0d,0x52,0x0a,0x6e, +0x75,0x6d,0x4f,0x75,0x74,0x70,0x75,0x74,0x73,0x12, +0x27,0x0a,0x01,0x61,0x18,0x04,0x20,0x01,0x28,0x0b, +0x32,0x19,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72,0x6f, +0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62,0x75, +0x66,0x4d,0x61,0x74,0x72,0x69,0x78,0x52,0x01,0x61, +0x12,0x27,0x0a,0x01,0x62,0x18,0x05,0x20,0x01,0x28, 0x0b,0x32,0x19,0x2e,0x77,0x70,0x69,0x2e,0x70,0x72, 0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f,0x62, 0x75,0x66,0x4d,0x61,0x74,0x72,0x69,0x78,0x52,0x01, -0x61,0x12,0x27,0x0a,0x01,0x62,0x18,0x05,0x20,0x01, +0x62,0x12,0x27,0x0a,0x01,0x63,0x18,0x06,0x20,0x01, 0x28,0x0b,0x32,0x19,0x2e,0x77,0x70,0x69,0x2e,0x70, 0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74,0x6f, 0x62,0x75,0x66,0x4d,0x61,0x74,0x72,0x69,0x78,0x52, -0x01,0x62,0x12,0x27,0x0a,0x01,0x63,0x18,0x06,0x20, +0x01,0x63,0x12,0x27,0x0a,0x01,0x64,0x18,0x07,0x20, 0x01,0x28,0x0b,0x32,0x19,0x2e,0x77,0x70,0x69,0x2e, 0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f,0x74, 0x6f,0x62,0x75,0x66,0x4d,0x61,0x74,0x72,0x69,0x78, -0x52,0x01,0x63,0x12,0x27,0x0a,0x01,0x64,0x18,0x07, -0x20,0x01,0x28,0x0b,0x32,0x19,0x2e,0x77,0x70,0x69, -0x2e,0x70,0x72,0x6f,0x74,0x6f,0x2e,0x50,0x72,0x6f, -0x74,0x6f,0x62,0x75,0x66,0x4d,0x61,0x74,0x72,0x69, -0x78,0x52,0x01,0x64,0x42,0x17,0x0a,0x15,0x6f,0x72, -0x67,0x2e,0x77,0x70,0x69,0x6c,0x69,0x62,0x2e,0x6d, -0x61,0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a, -0xd5,0x03,0x0a,0x06,0x12,0x04,0x00,0x00,0x10,0x01, -0x0a,0x08,0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12, -0x0a,0x08,0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12, -0x0a,0x09,0x0a,0x02,0x03,0x00,0x12,0x03,0x04,0x00, -0x17,0x0a,0x08,0x0a,0x01,0x08,0x12,0x03,0x06,0x00, -0x2e,0x0a,0x09,0x0a,0x02,0x08,0x01,0x12,0x03,0x06, -0x00,0x2e,0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04, -0x08,0x00,0x10,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00, -0x01,0x12,0x03,0x08,0x08,0x1c,0x0a,0x0b,0x0a,0x04, -0x04,0x00,0x02,0x00,0x12,0x03,0x09,0x02,0x18,0x0a, -0x0c,0x0a,0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03, -0x09,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, -0x00,0x01,0x12,0x03,0x09,0x09,0x13,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x00,0x03,0x12,0x03,0x09,0x16, -0x17,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12, -0x03,0x0a,0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x00, -0x02,0x01,0x05,0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c, -0x0a,0x05,0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x0a, -0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01, -0x03,0x12,0x03,0x0a,0x16,0x17,0x0a,0x0b,0x0a,0x04, -0x04,0x00,0x02,0x02,0x12,0x03,0x0b,0x02,0x19,0x0a, -0x0c,0x0a,0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03, -0x0b,0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, -0x02,0x01,0x12,0x03,0x0b,0x09,0x14,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x02,0x03,0x12,0x03,0x0b,0x17, -0x18,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x03,0x12, -0x03,0x0c,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x00, -0x02,0x03,0x06,0x12,0x03,0x0c,0x02,0x10,0x0a,0x0c, -0x0a,0x05,0x04,0x00,0x02,0x03,0x01,0x12,0x03,0x0c, -0x11,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x03, -0x03,0x12,0x03,0x0c,0x15,0x16,0x0a,0x0b,0x0a,0x04, -0x04,0x00,0x02,0x04,0x12,0x03,0x0d,0x02,0x17,0x0a, -0x0c,0x0a,0x05,0x04,0x00,0x02,0x04,0x06,0x12,0x03, -0x0d,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, -0x04,0x01,0x12,0x03,0x0d,0x11,0x12,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x04,0x03,0x12,0x03,0x0d,0x15, -0x16,0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x05,0x12, -0x03,0x0e,0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x00, -0x02,0x05,0x06,0x12,0x03,0x0e,0x02,0x10,0x0a,0x0c, -0x0a,0x05,0x04,0x00,0x02,0x05,0x01,0x12,0x03,0x0e, -0x11,0x12,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x05, -0x03,0x12,0x03,0x0e,0x15,0x16,0x0a,0x0b,0x0a,0x04, -0x04,0x00,0x02,0x06,0x12,0x03,0x0f,0x02,0x17,0x0a, -0x0c,0x0a,0x05,0x04,0x00,0x02,0x06,0x06,0x12,0x03, -0x0f,0x02,0x10,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, -0x06,0x01,0x12,0x03,0x0f,0x11,0x12,0x0a,0x0c,0x0a, -0x05,0x04,0x00,0x02,0x06,0x03,0x12,0x03,0x0f,0x15, -0x16,0x62,0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, +0x52,0x01,0x64,0x42,0x17,0x0a,0x15,0x6f,0x72,0x67, +0x2e,0x77,0x70,0x69,0x6c,0x69,0x62,0x2e,0x6d,0x61, +0x74,0x68,0x2e,0x70,0x72,0x6f,0x74,0x6f,0x4a,0x80, +0x06,0x0a,0x06,0x12,0x04,0x00,0x00,0x18,0x01,0x0a, +0x08,0x0a,0x01,0x0c,0x12,0x03,0x00,0x00,0x12,0x0a, +0x08,0x0a,0x01,0x02,0x12,0x03,0x02,0x00,0x12,0x0a, +0x09,0x0a,0x02,0x03,0x00,0x12,0x03,0x04,0x00,0x17, +0x0a,0x08,0x0a,0x01,0x08,0x12,0x03,0x06,0x00,0x2e, +0x0a,0x09,0x0a,0x02,0x08,0x01,0x12,0x03,0x06,0x00, +0x2e,0x0a,0x0a,0x0a,0x02,0x04,0x00,0x12,0x04,0x08, +0x00,0x0e,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x00,0x01, +0x12,0x03,0x08,0x08,0x17,0x0a,0x0b,0x0a,0x04,0x04, +0x00,0x02,0x00,0x12,0x03,0x09,0x02,0x1d,0x0a,0x0c, +0x0a,0x05,0x04,0x00,0x02,0x00,0x05,0x12,0x03,0x09, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x00, +0x01,0x12,0x03,0x09,0x09,0x18,0x0a,0x0c,0x0a,0x05, +0x04,0x00,0x02,0x00,0x03,0x12,0x03,0x09,0x1b,0x1c, +0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x01,0x12,0x03, +0x0a,0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, +0x01,0x05,0x12,0x03,0x0a,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x00,0x02,0x01,0x01,0x12,0x03,0x0a,0x09, +0x15,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x01,0x03, +0x12,0x03,0x0a,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04, +0x00,0x02,0x02,0x12,0x03,0x0b,0x02,0x1b,0x0a,0x0c, +0x0a,0x05,0x04,0x00,0x02,0x02,0x05,0x12,0x03,0x0b, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x02, +0x01,0x12,0x03,0x0b,0x09,0x16,0x0a,0x0c,0x0a,0x05, +0x04,0x00,0x02,0x02,0x03,0x12,0x03,0x0b,0x19,0x1a, +0x0a,0x0b,0x0a,0x04,0x04,0x00,0x02,0x03,0x12,0x03, +0x0c,0x02,0x1a,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02, +0x03,0x05,0x12,0x03,0x0c,0x02,0x08,0x0a,0x0c,0x0a, +0x05,0x04,0x00,0x02,0x03,0x01,0x12,0x03,0x0c,0x09, +0x15,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x03,0x03, +0x12,0x03,0x0c,0x18,0x19,0x0a,0x0b,0x0a,0x04,0x04, +0x00,0x02,0x04,0x12,0x03,0x0d,0x02,0x18,0x0a,0x0c, +0x0a,0x05,0x04,0x00,0x02,0x04,0x05,0x12,0x03,0x0d, +0x02,0x08,0x0a,0x0c,0x0a,0x05,0x04,0x00,0x02,0x04, +0x01,0x12,0x03,0x0d,0x09,0x13,0x0a,0x0c,0x0a,0x05, +0x04,0x00,0x02,0x04,0x03,0x12,0x03,0x0d,0x16,0x17, +0x0a,0x0a,0x0a,0x02,0x04,0x01,0x12,0x04,0x10,0x00, +0x18,0x01,0x0a,0x0a,0x0a,0x03,0x04,0x01,0x01,0x12, +0x03,0x10,0x08,0x1c,0x0a,0x0b,0x0a,0x04,0x04,0x01, +0x02,0x00,0x12,0x03,0x11,0x02,0x18,0x0a,0x0c,0x0a, +0x05,0x04,0x01,0x02,0x00,0x05,0x12,0x03,0x11,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x00,0x01, +0x12,0x03,0x11,0x09,0x13,0x0a,0x0c,0x0a,0x05,0x04, +0x01,0x02,0x00,0x03,0x12,0x03,0x11,0x16,0x17,0x0a, +0x0b,0x0a,0x04,0x04,0x01,0x02,0x01,0x12,0x03,0x12, +0x02,0x18,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01, +0x05,0x12,0x03,0x12,0x02,0x08,0x0a,0x0c,0x0a,0x05, +0x04,0x01,0x02,0x01,0x01,0x12,0x03,0x12,0x09,0x13, +0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x01,0x03,0x12, +0x03,0x12,0x16,0x17,0x0a,0x0b,0x0a,0x04,0x04,0x01, +0x02,0x02,0x12,0x03,0x13,0x02,0x19,0x0a,0x0c,0x0a, +0x05,0x04,0x01,0x02,0x02,0x05,0x12,0x03,0x13,0x02, +0x08,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x02,0x01, +0x12,0x03,0x13,0x09,0x14,0x0a,0x0c,0x0a,0x05,0x04, +0x01,0x02,0x02,0x03,0x12,0x03,0x13,0x17,0x18,0x0a, +0x0b,0x0a,0x04,0x04,0x01,0x02,0x03,0x12,0x03,0x14, +0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x03, +0x06,0x12,0x03,0x14,0x02,0x10,0x0a,0x0c,0x0a,0x05, +0x04,0x01,0x02,0x03,0x01,0x12,0x03,0x14,0x11,0x12, +0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x03,0x03,0x12, +0x03,0x14,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x01, +0x02,0x04,0x12,0x03,0x15,0x02,0x17,0x0a,0x0c,0x0a, +0x05,0x04,0x01,0x02,0x04,0x06,0x12,0x03,0x15,0x02, +0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x04,0x01, +0x12,0x03,0x15,0x11,0x12,0x0a,0x0c,0x0a,0x05,0x04, +0x01,0x02,0x04,0x03,0x12,0x03,0x15,0x15,0x16,0x0a, +0x0b,0x0a,0x04,0x04,0x01,0x02,0x05,0x12,0x03,0x16, +0x02,0x17,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x05, +0x06,0x12,0x03,0x16,0x02,0x10,0x0a,0x0c,0x0a,0x05, +0x04,0x01,0x02,0x05,0x01,0x12,0x03,0x16,0x11,0x12, +0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x05,0x03,0x12, +0x03,0x16,0x15,0x16,0x0a,0x0b,0x0a,0x04,0x04,0x01, +0x02,0x06,0x12,0x03,0x17,0x02,0x17,0x0a,0x0c,0x0a, +0x05,0x04,0x01,0x02,0x06,0x06,0x12,0x03,0x17,0x02, +0x10,0x0a,0x0c,0x0a,0x05,0x04,0x01,0x02,0x06,0x01, +0x12,0x03,0x17,0x11,0x12,0x0a,0x0c,0x0a,0x05,0x04, +0x01,0x02,0x06,0x03,0x12,0x03,0x17,0x15,0x16,0x62, +0x06,0x70,0x72,0x6f,0x74,0x6f,0x33, }; static const char file_name[] = "system.proto"; +static const char wpi_proto_ProtobufDCMotor_name[] = "wpi.proto.ProtobufDCMotor"; +std::string_view wpi_proto_ProtobufDCMotor::msg_name(void) noexcept { return wpi_proto_ProtobufDCMotor_name; } +pb_filedesc_t wpi_proto_ProtobufDCMotor::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } +PB_BIND(wpi_proto_ProtobufDCMotor, wpi_proto_ProtobufDCMotor, AUTO) + + static const char wpi_proto_ProtobufLinearSystem_name[] = "wpi.proto.ProtobufLinearSystem"; std::string_view wpi_proto_ProtobufLinearSystem::msg_name(void) noexcept { return wpi_proto_ProtobufLinearSystem_name; } pb_filedesc_t wpi_proto_ProtobufLinearSystem::file_descriptor(void) noexcept { return {::file_name, ::file_descriptor}; } @@ -104,3 +160,11 @@ PB_BIND(wpi_proto_ProtobufLinearSystem, wpi_proto_ProtobufLinearSystem, AUTO) +#ifndef PB_CONVERT_DOUBLE_FLOAT +/* On some platforms (such as AVR), double is really float. + * To be able to encode/decode double on these platforms, you need. + * to define PB_CONVERT_DOUBLE_FLOAT in pb.h or compiler command line. + */ +PB_STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) +#endif + diff --git a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/system.npb.h b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/system.npb.h index 2f7b40f3d9..043f94f430 100644 --- a/wpimath/src/generated/main/native/cpp/wpimath/protobuf/system.npb.h +++ b/wpimath/src/generated/main/native/cpp/wpimath/protobuf/system.npb.h @@ -16,6 +16,18 @@ #endif /* Struct definitions */ +typedef struct _wpi_proto_ProtobufDCMotor { + static const pb_msgdesc_t* msg_descriptor(void) noexcept; + static std::string_view msg_name(void) noexcept; + static pb_filedesc_t file_descriptor(void) noexcept; + + double nominal_voltage; + double stall_torque; + double stall_current; + double free_current; + double free_speed; +} wpi_proto_ProtobufDCMotor; + typedef struct _wpi_proto_ProtobufLinearSystem { static const pb_msgdesc_t* msg_descriptor(void) noexcept; static std::string_view msg_name(void) noexcept; @@ -32,10 +44,17 @@ typedef struct _wpi_proto_ProtobufLinearSystem { /* Initializer values for message structs */ +#define wpi_proto_ProtobufDCMotor_init_default {0, 0, 0, 0, 0} #define wpi_proto_ProtobufLinearSystem_init_default {0, 0, 0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}} +#define wpi_proto_ProtobufDCMotor_init_zero {0, 0, 0, 0, 0} #define wpi_proto_ProtobufLinearSystem_init_zero {0, 0, 0, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}, {{NULL}, NULL}} /* Field tags (for use in manual encoding/decoding) */ +#define wpi_proto_ProtobufDCMotor_nominal_voltage_tag 1 +#define wpi_proto_ProtobufDCMotor_stall_torque_tag 2 +#define wpi_proto_ProtobufDCMotor_stall_current_tag 3 +#define wpi_proto_ProtobufDCMotor_free_current_tag 4 +#define wpi_proto_ProtobufDCMotor_free_speed_tag 5 #define wpi_proto_ProtobufLinearSystem_num_states_tag 1 #define wpi_proto_ProtobufLinearSystem_num_inputs_tag 2 #define wpi_proto_ProtobufLinearSystem_num_outputs_tag 3 @@ -45,6 +64,15 @@ typedef struct _wpi_proto_ProtobufLinearSystem { #define wpi_proto_ProtobufLinearSystem_d_tag 7 /* Struct field encoding specification for nanopb */ +#define wpi_proto_ProtobufDCMotor_FIELDLIST(X, a) \ +X(a, STATIC, SINGULAR, DOUBLE, nominal_voltage, 1) \ +X(a, STATIC, SINGULAR, DOUBLE, stall_torque, 2) \ +X(a, STATIC, SINGULAR, DOUBLE, stall_current, 3) \ +X(a, STATIC, SINGULAR, DOUBLE, free_current, 4) \ +X(a, STATIC, SINGULAR, DOUBLE, free_speed, 5) +#define wpi_proto_ProtobufDCMotor_CALLBACK NULL +#define wpi_proto_ProtobufDCMotor_DEFAULT NULL + #define wpi_proto_ProtobufLinearSystem_FIELDLIST(X, a_) \ X(a_, STATIC, SINGULAR, UINT32, num_states, 1) \ X(a_, STATIC, SINGULAR, UINT32, num_inputs, 2) \ @@ -62,6 +90,8 @@ X(a_, CALLBACK, OPTIONAL, MESSAGE, d, 7) /* Maximum encoded size of messages (where known) */ /* wpi_proto_ProtobufLinearSystem_size depends on runtime parameters */ +#define WPI_PROTO_SYSTEM_NPB_H_MAX_SIZE wpi_proto_ProtobufDCMotor_size +#define wpi_proto_ProtobufDCMotor_size 45 #endif diff --git a/wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveAccelerationLimiter.java b/wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveAccelerationLimiter.java index 59c9454de8..01765d4032 100644 --- a/wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveAccelerationLimiter.java +++ b/wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveAccelerationLimiter.java @@ -16,7 +16,7 @@ import org.wpilib.math.util.Nat; * Filters the provided voltages to limit a differential drive's linear and angular acceleration. * *

The differential drive model can be created via the functions in {@link - * org.wpilib.math.system.plant.LinearSystemId}. + * org.wpilib.math.system.Models}. */ public class DifferentialDriveAccelerationLimiter { private final LinearSystem m_system; diff --git a/wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveFeedforward.java b/wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveFeedforward.java index e5dfeeaf07..d15c54388a 100644 --- a/wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveFeedforward.java +++ b/wpimath/src/main/java/org/wpilib/math/controller/DifferentialDriveFeedforward.java @@ -9,7 +9,7 @@ import org.wpilib.math.controller.struct.DifferentialDriveFeedforwardStruct; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N2; import org.wpilib.math.system.LinearSystem; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.util.struct.StructSerializable; @@ -41,7 +41,7 @@ public class DifferentialDriveFeedforward implements ProtobufSerializable, Struc */ public DifferentialDriveFeedforward( double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) { - // See LinearSystemId.identifyDrivetrainSystem(double, double, double, double, double) + // See Models.differentialDriveFromSysId(double, double, double, double, double) this(kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth); } @@ -55,7 +55,7 @@ public class DifferentialDriveFeedforward implements ProtobufSerializable, Struc */ public DifferentialDriveFeedforward( double kVLinear, double kALinear, double kVAngular, double kAAngular) { - m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular); + m_plant = Models.differentialDriveFromSysId(kVLinear, kALinear, kVAngular, kAAngular); m_kVLinear = kVLinear; m_kALinear = kALinear; m_kVAngular = kVAngular; diff --git a/wpimath/src/main/java/org/wpilib/math/system/plant/DCMotor.java b/wpimath/src/main/java/org/wpilib/math/system/DCMotor.java similarity index 98% rename from wpimath/src/main/java/org/wpilib/math/system/plant/DCMotor.java rename to wpimath/src/main/java/org/wpilib/math/system/DCMotor.java index 5eb7fecd3e..df339f226b 100644 --- a/wpimath/src/main/java/org/wpilib/math/system/plant/DCMotor.java +++ b/wpimath/src/main/java/org/wpilib/math/system/DCMotor.java @@ -2,10 +2,10 @@ // 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. -package org.wpilib.math.system.plant; +package org.wpilib.math.system; -import org.wpilib.math.system.plant.proto.DCMotorProto; -import org.wpilib.math.system.plant.struct.DCMotorStruct; +import org.wpilib.math.system.proto.DCMotorProto; +import org.wpilib.math.system.struct.DCMotorStruct; import org.wpilib.math.util.Units; import org.wpilib.util.protobuf.ProtobufSerializable; import org.wpilib.util.struct.StructSerializable; diff --git a/wpimath/src/main/java/org/wpilib/math/system/Models.java b/wpimath/src/main/java/org/wpilib/math/system/Models.java new file mode 100644 index 0000000000..7916571871 --- /dev/null +++ b/wpimath/src/main/java/org/wpilib/math/system/Models.java @@ -0,0 +1,370 @@ +// 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. + +package org.wpilib.math.system; + +import org.wpilib.math.linalg.MatBuilder; +import org.wpilib.math.numbers.N1; +import org.wpilib.math.numbers.N2; +import org.wpilib.math.util.Nat; + +/** Linear system factories. */ +public final class Models { + private Models() { + // Utility class + } + + /** + * Creates a flywheel state-space model from physical constants. + * + *

The states are [angular velocity], the inputs are [voltage], and the outputs are [angular + * velocity]. + * + * @param motor The motor (or gearbox) attached to the flywheel. + * @param J The moment of inertia J of the flywheel. + * @param gearing Gear ratio from motor to flywheel (greater than 1 is a reduction). + * @return Flywheel state-space model. + * @throws IllegalArgumentException if J <= 0 or gearing <= 0. + */ + public static LinearSystem flywheelFromPhysicalConstants( + DCMotor motor, double J, double gearing) { + if (J <= 0.0) { + throw new IllegalArgumentException("J must be greater than zero."); + } + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); + } + + var A = + MatBuilder.fill( + Nat.N1(), Nat.N1(), -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)); + var B = MatBuilder.fill(Nat.N1(), Nat.N1(), gearing * motor.Kt / (motor.R * J)); + var C = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0); + var D = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0); + + return new LinearSystem<>(A, B, C, D); + } + + /** + * Creates a flywheel state-space model from SysId constants kᵥ (V/(rad/s)) and kₐ (V/(rad/s²)) + * from the feedforward model u = kᵥv + kₐa. + * + *

The states are [velocity], the inputs are [voltage], and the outputs are [velocity]. + * + * @param kV The velocity gain, in V/(rad/s). + * @param kA The acceleration gain, in V/(rad/s²). + * @return Flywheel state-space model. + * @throws IllegalArgumentException if kV < 0 or kA <= 0. + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid + */ + public static LinearSystem flywheelFromSysId(double kV, double kA) { + if (kV < 0.0) { + throw new IllegalArgumentException("Kv must be greater than or equal to zero."); + } + if (kA <= 0.0) { + throw new IllegalArgumentException("Ka must be greater than zero."); + } + + var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kV / kA); + var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / kA); + var C = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0); + var D = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.0); + + return new LinearSystem<>(A, B, C, D); + } + + /** + * Creates an elevator state-space model from physical constants. + * + *

The states are [position, velocity], the inputs are [voltage], and the outputs are + * [position, velocity]. + * + * @param motor The motor (or gearbox) attached to the carriage. + * @param mass The mass of the elevator carriage, in kilograms. + * @param radius The radius of the elevator's driving drum, in meters. + * @param gearing Gear ratio from motor to carriage (greater than 1 is a reduction). + * @return Elevator state-space model. + * @throws IllegalArgumentException if mass <= 0, radius <= 0, or gearing <= 0. + */ + public static LinearSystem elevatorFromPhysicalConstants( + DCMotor motor, double mass, double radius, double gearing) { + if (mass <= 0.0) { + throw new IllegalArgumentException("mass must be greater than zero."); + } + if (radius <= 0.0) { + throw new IllegalArgumentException("radius must be greater than zero."); + } + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); + } + + var A = + MatBuilder.fill( + Nat.N2(), + Nat.N2(), + 0.0, + 1.0, + 0.0, + -Math.pow(gearing, 2) * motor.Kt / (motor.R * Math.pow(radius, 2) * mass * motor.Kv)); + var B = + MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, gearing * motor.Kt / (motor.R * radius * mass)); + var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); + var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0); + + return new LinearSystem<>(A, B, C, D); + } + + /** + * Creates an elevator state-space model from SysId constants kᵥ (V/(m/s)) and kₐ (V/(m/s²)) from + * the feedforward model u = kᵥv + kₐa. + * + *

The states are [position, velocity], the inputs are [voltage], and the outputs are + * [position, velocity]. + * + * @param kV The velocity gain, in V/(m/s). + * @param kA The acceleration gain, in V/(m/s²). + * @return Elevator state-space model. + * @throws IllegalArgumentException if kV < 0 or kA <= 0. + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid + */ + public static LinearSystem elevatorFromSysId(double kV, double kA) { + if (kV < 0.0) { + throw new IllegalArgumentException("Kv must be greater than or equal to zero."); + } + if (kA <= 0.0) { + throw new IllegalArgumentException("Ka must be greater than zero."); + } + + var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA); + var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 1.0 / kA); + var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); + var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0); + + return new LinearSystem<>(A, B, C, D); + } + + /** + * Create a single-jointed arm state-space model from physical constants. + * + *

The states are [angle, angular velocity], the inputs are [voltage], and the outputs are + * [angle, angular velocity]. + * + * @param motor The motor (or gearbox) attached to the arm. + * @param J The moment of inertia J of the arm. + * @param gearing Gear ratio from motor to arm (greater than 1 is a reduction). + * @return Single-jointed arm state-space model. + * @throws IllegalArgumentException if J <= 0 or gearing <= 0. + */ + public static LinearSystem singleJointedArmFromPhysicalConstants( + DCMotor motor, double J, double gearing) { + if (J <= 0.0) { + throw new IllegalArgumentException("J must be greater than zero."); + } + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); + } + + var A = + MatBuilder.fill( + Nat.N2(), + Nat.N2(), + 0.0, + 1.0, + 0.0, + -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)); + var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, gearing * motor.Kt / (motor.R * J)); + var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); + var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0); + + return new LinearSystem<>(A, B, C, D); + } + + /** + * Creates a single-jointed arm state-space model from SysId constants kᵥ (V/(rad/s)) and kₐ + * (V/(rad/s²)) from the feedforward model u = kᵥv + kₐa. + * + *

The states are [position, velocity], the inputs are [voltage], and the outputs are + * [position, velocity]. + * + * @param kV The velocity gain, in volts/(unit/sec). + * @param kA The acceleration gain, in volts/(unit/sec²). + * @return Single-jointed arm state-space model. + * @throws IllegalArgumentException if kV < 0 or kA <= 0. + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid + */ + public static LinearSystem singleJointedArmFromSysId(double kV, double kA) { + if (kV < 0.0) { + throw new IllegalArgumentException("Kv must be greater than or equal to zero."); + } + if (kA <= 0.0) { + throw new IllegalArgumentException("Ka must be greater than zero."); + } + + var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA); + var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 1.0 / kA); + var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); + var D = MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 0.0); + + return new LinearSystem<>(A, B, C, D); + } + + /** + * Creates a differential drive state-space model from physical constants. + * + *

The states are [left velocity, right velocity], the inputs are [left voltage, right + * voltage], and the outputs are [left velocity, right velocity]. + * + * @param motor The motor (or gearbox) driving the drivetrain. + * @param mass The mass of the robot in kilograms. + * @param r The radius of the wheels in meters. + * @param rb The radius of the base (half of the trackwidth), in meters. + * @param J The moment of inertia of the robot. + * @param gearing Gear ratio from motor to wheel (greater than 1 is a reduction). + * @return Differential drive state-space model. + * @throws IllegalArgumentException if mass <= 0, r <= 0, rb <= 0, J <= 0, or gearing + * <= 0. + */ + public static LinearSystem differentialDriveFromPhysicalConstants( + DCMotor motor, double mass, double r, double rb, double J, double gearing) { + if (mass <= 0.0) { + throw new IllegalArgumentException("mass must be greater than zero."); + } + if (r <= 0.0) { + throw new IllegalArgumentException("r must be greater than zero."); + } + if (rb <= 0.0) { + throw new IllegalArgumentException("rb must be greater than zero."); + } + if (J <= 0.0) { + throw new IllegalArgumentException("J must be greater than zero."); + } + if (gearing <= 0.0) { + throw new IllegalArgumentException("gearing must be greater than zero."); + } + + double C1 = -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * Math.pow(r, 2)); + double C2 = gearing * motor.Kt / (motor.R * r); + + var A = + MatBuilder.fill( + Nat.N2(), + Nat.N2(), + (1 / mass + Math.pow(rb, 2) / J) * C1, + (1 / mass - Math.pow(rb, 2) / J) * C1, + (1 / mass - Math.pow(rb, 2) / J) * C1, + (1 / mass + Math.pow(rb, 2) / J) * C1); + var B = + MatBuilder.fill( + Nat.N2(), + Nat.N2(), + (1 / mass + Math.pow(rb, 2) / J) * C2, + (1 / mass - Math.pow(rb, 2) / J) * C2, + (1 / mass - Math.pow(rb, 2) / J) * C2, + (1 / mass + Math.pow(rb, 2) / J) * C2); + var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); + var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0); + + return new LinearSystem<>(A, B, C, D); + } + + /** + * Creates a differential drive state-space model from SysId constants kᵥ and kₐ in both linear + * {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s), (V/(rad/s²))} cases. + * + *

The states are [left velocity, right velocity], the inputs are [left voltage, right + * voltage], and the outputs are [left velocity, right velocity]. + * + * @param kVLinear The linear velocity gain in volts per (meters per second). + * @param kALinear The linear acceleration gain in volts per (meters per second squared). + * @param kVAngular The angular velocity gain in volts per (meters per second). + * @param kAAngular The angular acceleration gain in volts per (meters per second squared). + * @return Differential drive state-space model. + * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, or + * kAAngular <= 0. + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid + */ + public static LinearSystem differentialDriveFromSysId( + double kVLinear, double kALinear, double kVAngular, double kAAngular) { + if (kVLinear <= 0.0) { + throw new IllegalArgumentException("Kv,linear must be greater than zero."); + } + if (kALinear <= 0.0) { + throw new IllegalArgumentException("Ka,linear must be greater than zero."); + } + if (kVAngular <= 0.0) { + throw new IllegalArgumentException("Kv,angular must be greater than zero."); + } + if (kAAngular <= 0.0) { + throw new IllegalArgumentException("Ka,angular must be greater than zero."); + } + + double A1 = -0.5 * (kVLinear / kALinear + kVAngular / kAAngular); + double A2 = -0.5 * (kVLinear / kALinear - kVAngular / kAAngular); + double B1 = 0.5 / kALinear + 0.5 / kAAngular; + double B2 = 0.5 / kALinear - 0.5 / kAAngular; + + var A = MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1); + var B = MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1); + var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); + var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0); + + return new LinearSystem<>(A, B, C, D); + } + + /** + * Creates a differential drive state-space model from SysId constants kᵥ and kₐ in both linear + * {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s), (V/(rad/s²))} cases. + * + *

The states are [left velocity, right velocity], the inputs are [left voltage, right + * voltage], and the outputs are [left velocity, right velocity]. + * + * @param kVLinear The linear velocity gain in volts per (meters per second). + * @param kALinear The linear acceleration gain in volts per (meters per second squared). + * @param kVAngular The angular velocity gain in volts per (radians per second). + * @param kAAngular The angular acceleration gain in volts per (radians per second squared). + * @param trackwidth The distance between the differential drive's left and right wheels, in + * meters. + * @return Differential drive state-space model. + * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, + * kAAngular <= 0, or trackwidth <= 0. + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid + */ + public static LinearSystem differentialDriveFromSysId( + double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) { + if (kVLinear <= 0.0) { + throw new IllegalArgumentException("Kv,linear must be greater than zero."); + } + if (kALinear <= 0.0) { + throw new IllegalArgumentException("Ka,linear must be greater than zero."); + } + if (kVAngular <= 0.0) { + throw new IllegalArgumentException("Kv,angular must be greater than zero."); + } + if (kAAngular <= 0.0) { + throw new IllegalArgumentException("Ka,angular must be greater than zero."); + } + if (trackwidth <= 0.0) { + throw new IllegalArgumentException("r must be greater than zero."); + } + + // We want to find a factor to include in Kv,angular that will convert + // `u = Kv,angular omega` to `u = Kv,angular v`. + // + // v = omega r + // omega = v/r + // omega = 1/r v + // omega = 1/(trackwidth/2) v + // omega = 2/trackwidth v + // + // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s) + // to V/(m/s). + return differentialDriveFromSysId( + kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth); + } +} diff --git a/wpimath/src/main/java/org/wpilib/math/system/plant/LinearSystemId.java b/wpimath/src/main/java/org/wpilib/math/system/plant/LinearSystemId.java deleted file mode 100644 index ac34f8d514..0000000000 --- a/wpimath/src/main/java/org/wpilib/math/system/plant/LinearSystemId.java +++ /dev/null @@ -1,390 +0,0 @@ -// 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. - -package org.wpilib.math.system.plant; - -import org.wpilib.math.linalg.MatBuilder; -import org.wpilib.math.linalg.Matrix; -import org.wpilib.math.linalg.VecBuilder; -import org.wpilib.math.numbers.N1; -import org.wpilib.math.numbers.N2; -import org.wpilib.math.system.LinearSystem; -import org.wpilib.math.util.Nat; - -/** Linear system ID utility functions. */ -public final class LinearSystemId { - private LinearSystemId() { - // Utility class - } - - /** - * Create a state-space model of an elevator system. The states of the system are [position, - * velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ. - * - * @param motor The motor (or gearbox) attached to the carriage. - * @param mass The mass of the elevator carriage, in kilograms. - * @param radius The radius of the elevator's driving drum, in meters. - * @param gearing The reduction between motor and drum, as a ratio of output to input. - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if mass <= 0, radius <= 0, or gearing <= 0. - */ - public static LinearSystem createElevatorSystem( - DCMotor motor, double mass, double radius, double gearing) { - if (mass <= 0.0) { - throw new IllegalArgumentException("mass must be greater than zero."); - } - if (radius <= 0.0) { - throw new IllegalArgumentException("radius must be greater than zero."); - } - if (gearing <= 0) { - throw new IllegalArgumentException("gearing must be greater than zero."); - } - - return new LinearSystem<>( - MatBuilder.fill( - Nat.N2(), - Nat.N2(), - 0, - 1, - 0, - -Math.pow(gearing, 2) * motor.Kt / (motor.R * radius * radius * mass * motor.Kv)), - VecBuilder.fill(0, gearing * motor.Kt / (motor.R * radius * mass)), - Matrix.eye(Nat.N2()), - new Matrix<>(Nat.N2(), Nat.N1())); - } - - /** - * Create a state-space model of a flywheel system. The states of the system are [angular - * velocity], inputs are [voltage], and outputs are [angular velocity]. - * - * @param motor The motor (or gearbox) attached to the flywheel. - * @param J The moment of inertia J of the flywheel in kg-m². - * @param gearing The reduction between motor and drum, as a ratio of output to input. - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if J <= 0 or gearing <= 0. - */ - public static LinearSystem createFlywheelSystem( - DCMotor motor, double J, double gearing) { - if (J <= 0.0) { - throw new IllegalArgumentException("J must be greater than zero."); - } - if (gearing <= 0.0) { - throw new IllegalArgumentException("gearing must be greater than zero."); - } - - return new LinearSystem<>( - VecBuilder.fill(-gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)), - VecBuilder.fill(gearing * motor.Kt / (motor.R * J)), - Matrix.eye(Nat.N1()), - new Matrix<>(Nat.N1(), Nat.N1())); - } - - /** - * Create a state-space model of a DC motor system. The states of the system are [angular - * position, angular velocity]ᵀ, inputs are [voltage], and outputs are [angular position, angular - * velocity]ᵀ. - * - * @param motor The motor (or gearbox) attached to system. - * @param J The moment of inertia J of the DC motor in kg-m². - * @param gearing The reduction between motor and drum, as a ratio of output to input. - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if J <= 0 or gearing <= 0. - */ - public static LinearSystem createDCMotorSystem( - DCMotor motor, double J, double gearing) { - if (J <= 0.0) { - throw new IllegalArgumentException("J must be greater than zero."); - } - if (gearing <= 0.0) { - throw new IllegalArgumentException("gearing must be greater than zero."); - } - - return new LinearSystem<>( - MatBuilder.fill( - Nat.N2(), Nat.N2(), 0, 1, 0, -gearing * gearing * motor.Kt / (motor.Kv * motor.R * J)), - VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)), - Matrix.eye(Nat.N2()), - new Matrix<>(Nat.N2(), Nat.N1())); - } - - /** - * Create a state-space model of a DC motor system from its kV (volts/(unit/sec)) and kA - * (volts/(unit/sec²)). These constants can be found using SysId. the states of the system are - * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position]. - * - *

The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the - * {@link org.wpilib.math.util.Units} class for converting between unit types. - * - *

The parameters provided by the user are from this feedforward model: - * - *

u = K_v v + K_a a - * - * @param kV The velocity gain, in volts/(unit/sec) - * @param kA The acceleration gain, in volts/(unit/sec²) - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if kV < 0 or kA <= 0. - * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid - */ - public static LinearSystem createDCMotorSystem(double kV, double kA) { - if (kV < 0.0) { - throw new IllegalArgumentException("Kv must be greater than or equal to zero."); - } - if (kA <= 0.0) { - throw new IllegalArgumentException("Ka must be greater than zero."); - } - - return new LinearSystem<>( - MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kV / kA), - VecBuilder.fill(0, 1 / kA), - Matrix.eye(Nat.N2()), - new Matrix<>(Nat.N2(), Nat.N1())); - } - - /** - * Create a state-space model of a differential drive drivetrain. In this model, the states are - * [left velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are - * [left velocity, right velocity]ᵀ. - * - * @param motor The motor (or gearbox) driving the drivetrain. - * @param mass The mass of the robot in kilograms. - * @param r The radius of the wheels in meters. - * @param rb The radius of the base (half the trackwidth) in meters. - * @param J The moment of inertia of the robot in kg-m². - * @param gearing The gearing reduction as output over input. - * @return A LinearSystem representing a differential drivetrain. - * @throws IllegalArgumentException if m <= 0, r <= 0, rb <= 0, J <= 0, or gearing - * <= 0. - */ - public static LinearSystem createDrivetrainVelocitySystem( - DCMotor motor, double mass, double r, double rb, double J, double gearing) { - if (mass <= 0.0) { - throw new IllegalArgumentException("mass must be greater than zero."); - } - if (r <= 0.0) { - throw new IllegalArgumentException("r must be greater than zero."); - } - if (rb <= 0.0) { - throw new IllegalArgumentException("rb must be greater than zero."); - } - if (J <= 0.0) { - throw new IllegalArgumentException("J must be greater than zero."); - } - if (gearing <= 0.0) { - throw new IllegalArgumentException("gearing must be greater than zero."); - } - - var C1 = -(gearing * gearing) * motor.Kt / (motor.Kv * motor.R * r * r); - var C2 = gearing * motor.Kt / (motor.R * r); - - final double C3 = 1 / mass + rb * rb / J; - final double C4 = 1 / mass - rb * rb / J; - var A = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C1, C4 * C1, C4 * C1, C3 * C1); - var B = MatBuilder.fill(Nat.N2(), Nat.N2(), C3 * C2, C4 * C2, C4 * C2, C3 * C2); - var C = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0); - var D = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0); - - return new LinearSystem<>(A, B, C, D); - } - - /** - * Create a state-space model of a single jointed arm system. The states of the system are [angle, - * angular velocity]ᵀ, inputs are [voltage], and outputs are [angle, angular velocity]ᵀ. - * - * @param motor The motor (or gearbox) attached to the arm. - * @param J The moment of inertia J of the arm in kg-m². - * @param gearing The gearing between the motor and arm, in output over input. Most of the time - * this will be greater than 1. - * @return A LinearSystem representing the given characterized constants. - */ - public static LinearSystem createSingleJointedArmSystem( - DCMotor motor, double J, double gearing) { - if (J <= 0.0) { - throw new IllegalArgumentException("J must be greater than zero."); - } - if (gearing <= 0.0) { - throw new IllegalArgumentException("gearing must be greater than zero."); - } - - return new LinearSystem<>( - MatBuilder.fill( - Nat.N2(), - Nat.N2(), - 0, - 1, - 0, - -Math.pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)), - VecBuilder.fill(0, gearing * motor.Kt / (motor.R * J)), - Matrix.eye(Nat.N2()), - new Matrix<>(Nat.N2(), Nat.N1())); - } - - /** - * Create a state-space model for a 1 DOF velocity system from its kV (volts/(unit/sec)) and kA - * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are - * [velocity], inputs are [voltage], and outputs are [velocity]. - * - *

The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the - * {@link org.wpilib.math.util.Units} class for converting between unit types. - * - *

The parameters provided by the user are from this feedforward model: - * - *

u = K_v v + K_a a - * - * @param kV The velocity gain, in volts/(unit/sec) - * @param kA The acceleration gain, in volts/(unit/sec²) - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if kV < 0 or kA <= 0. - * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid - */ - public static LinearSystem identifyVelocitySystem(double kV, double kA) { - if (kV < 0.0) { - throw new IllegalArgumentException("Kv must be greater than or equal to zero."); - } - if (kA <= 0.0) { - throw new IllegalArgumentException("Ka must be greater than zero."); - } - - return new LinearSystem<>( - VecBuilder.fill(-kV / kA), - VecBuilder.fill(1.0 / kA), - VecBuilder.fill(1.0), - VecBuilder.fill(0.0)); - } - - /** - * Create a state-space model for a 1 DOF position system from its kV (volts/(unit/sec)) and kA - * (volts/(unit/sec²). These constants cam be found using SysId. The states of the system are - * [position, velocity]ᵀ, inputs are [voltage], and outputs are [position, velocity]ᵀ. - * - *

The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the - * {@link org.wpilib.math.util.Units} class for converting between unit types. - * - *

The parameters provided by the user are from this feedforward model: - * - *

u = K_v v + K_a a - * - * @param kV The velocity gain, in volts/(unit/sec) - * @param kA The acceleration gain, in volts/(unit/sec²) - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if kV < 0 or kA <= 0. - * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid - */ - public static LinearSystem identifyPositionSystem(double kV, double kA) { - if (kV < 0.0) { - throw new IllegalArgumentException("Kv must be greater than or equal to zero."); - } - if (kA <= 0.0) { - throw new IllegalArgumentException("Ka must be greater than zero."); - } - - return new LinearSystem<>( - MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kV / kA), - VecBuilder.fill(0.0, 1.0 / kA), - Matrix.eye(Nat.N2()), - new Matrix<>(Nat.N2(), Nat.N1())); - } - - /** - * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear - * {(volts/(meter/sec), (volts/(meter/sec²))} and angular {(volts/(radian/sec)), - * (volts/(radian/sec²))} cases. These constants can be found using SysId. - * - *

States: [[left velocity], [right velocity]]
- * Inputs: [[left voltage], [right voltage]]
- * Outputs: [[left velocity], [right velocity]] - * - * @param kVLinear The linear velocity gain in volts per (meters per second). - * @param kALinear The linear acceleration gain in volts per (meters per second squared). - * @param kVAngular The angular velocity gain in volts per (meters per second). - * @param kAAngular The angular acceleration gain in volts per (meters per second squared). - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, or - * kAAngular <= 0. - * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid - */ - public static LinearSystem identifyDrivetrainSystem( - double kVLinear, double kALinear, double kVAngular, double kAAngular) { - if (kVLinear <= 0.0) { - throw new IllegalArgumentException("Kv,linear must be greater than zero."); - } - if (kALinear <= 0.0) { - throw new IllegalArgumentException("Ka,linear must be greater than zero."); - } - if (kVAngular <= 0.0) { - throw new IllegalArgumentException("Kv,angular must be greater than zero."); - } - if (kAAngular <= 0.0) { - throw new IllegalArgumentException("Ka,angular must be greater than zero."); - } - - final double A1 = 0.5 * -(kVLinear / kALinear + kVAngular / kAAngular); - final double A2 = 0.5 * -(kVLinear / kALinear - kVAngular / kAAngular); - final double B1 = 0.5 * (1.0 / kALinear + 1.0 / kAAngular); - final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular); - - return new LinearSystem<>( - MatBuilder.fill(Nat.N2(), Nat.N2(), A1, A2, A2, A1), - MatBuilder.fill(Nat.N2(), Nat.N2(), B1, B2, B2, B1), - MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1), - MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0)); - } - - /** - * Identify a differential drive drivetrain given the drivetrain's kV and kA in both linear - * {(volts/(meter/sec)), (volts/(meter/sec²))} and angular {(volts/(radian/sec)), - * (volts/(radian/sec²))} cases. This can be found using SysId. - * - *

States: [[left velocity], [right velocity]]
- * Inputs: [[left voltage], [right voltage]]
- * Outputs: [[left velocity], [right velocity]] - * - * @param kVLinear The linear velocity gain in volts per (meters per second). - * @param kALinear The linear acceleration gain in volts per (meters per second squared). - * @param kVAngular The angular velocity gain in volts per (radians per second). - * @param kAAngular The angular acceleration gain in volts per (radians per second squared). - * @param trackwidth The distance between the differential drive's left and right wheels, in - * meters. - * @return A LinearSystem representing the given characterized constants. - * @throws IllegalArgumentException if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, - * kAAngular <= 0, or trackwidth <= 0. - * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid - */ - public static LinearSystem identifyDrivetrainSystem( - double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) { - if (kVLinear <= 0.0) { - throw new IllegalArgumentException("Kv,linear must be greater than zero."); - } - if (kALinear <= 0.0) { - throw new IllegalArgumentException("Ka,linear must be greater than zero."); - } - if (kVAngular <= 0.0) { - throw new IllegalArgumentException("Kv,angular must be greater than zero."); - } - if (kAAngular <= 0.0) { - throw new IllegalArgumentException("Ka,angular must be greater than zero."); - } - if (trackwidth <= 0.0) { - throw new IllegalArgumentException("trackwidth must be greater than zero."); - } - - // We want to find a factor to include in Kv,angular that will convert - // `u = Kv,angular omega` to `u = Kv,angular v`. - // - // v = omega r - // omega = v/r - // omega = 1/r v - // omega = 1/(trackwidth/2) v - // omega = 2/trackwidth v - // - // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s) - // to V/(m/s). - return identifyDrivetrainSystem( - kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth); - } -} diff --git a/wpimath/src/main/java/org/wpilib/math/system/plant/proto/DCMotorProto.java b/wpimath/src/main/java/org/wpilib/math/system/proto/DCMotorProto.java similarity index 89% rename from wpimath/src/main/java/org/wpilib/math/system/plant/proto/DCMotorProto.java rename to wpimath/src/main/java/org/wpilib/math/system/proto/DCMotorProto.java index ec596a1b25..bd183b7395 100644 --- a/wpimath/src/main/java/org/wpilib/math/system/plant/proto/DCMotorProto.java +++ b/wpimath/src/main/java/org/wpilib/math/system/proto/DCMotorProto.java @@ -2,10 +2,10 @@ // 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. -package org.wpilib.math.system.plant.proto; +package org.wpilib.math.system.proto; -import org.wpilib.math.proto.Plant.ProtobufDCMotor; -import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.proto.System.ProtobufDCMotor; +import org.wpilib.math.system.DCMotor; import org.wpilib.util.protobuf.Protobuf; import us.hebi.quickbuf.Descriptors.Descriptor; diff --git a/wpimath/src/main/java/org/wpilib/math/system/plant/struct/DCMotorStruct.java b/wpimath/src/main/java/org/wpilib/math/system/struct/DCMotorStruct.java similarity index 93% rename from wpimath/src/main/java/org/wpilib/math/system/plant/struct/DCMotorStruct.java rename to wpimath/src/main/java/org/wpilib/math/system/struct/DCMotorStruct.java index 93b6c49280..b965065741 100644 --- a/wpimath/src/main/java/org/wpilib/math/system/plant/struct/DCMotorStruct.java +++ b/wpimath/src/main/java/org/wpilib/math/system/struct/DCMotorStruct.java @@ -2,10 +2,10 @@ // 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. -package org.wpilib.math.system.plant.struct; +package org.wpilib.math.system.struct; import java.nio.ByteBuffer; -import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.system.DCMotor; import org.wpilib.util.struct.Struct; public class DCMotorStruct implements Struct { diff --git a/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp b/wpimath/src/main/native/cpp/system/proto/DCMotorProto.cpp similarity index 92% rename from wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp rename to wpimath/src/main/native/cpp/system/proto/DCMotorProto.cpp index 2a45b28086..901e2a873e 100644 --- a/wpimath/src/main/native/cpp/system/plant/proto/DCMotorProto.cpp +++ b/wpimath/src/main/native/cpp/system/proto/DCMotorProto.cpp @@ -2,11 +2,11 @@ // 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 "wpi/math/system/plant/proto/DCMotorProto.hpp" +#include "wpi/math/system/proto/DCMotorProto.hpp" #include -#include "wpimath/protobuf/plant.npb.h" +#include "wpimath/protobuf/system.npb.h" std::optional wpi::util::Protobuf::Unpack(InputStream& stream) { diff --git a/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp b/wpimath/src/main/native/cpp/system/struct/DCMotorStruct.cpp similarity index 96% rename from wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp rename to wpimath/src/main/native/cpp/system/struct/DCMotorStruct.cpp index d9f0202a0b..685bd5bbb7 100644 --- a/wpimath/src/main/native/cpp/system/plant/struct/DCMotorStruct.cpp +++ b/wpimath/src/main/native/cpp/system/struct/DCMotorStruct.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 "wpi/math/system/plant/struct/DCMotorStruct.hpp" +#include "wpi/math/system/struct/DCMotorStruct.hpp" namespace { constexpr size_t kNominalVoltageOff = 0; diff --git a/wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveAccelerationLimiter.hpp b/wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveAccelerationLimiter.hpp index 0c9eee3832..da78963ee1 100644 --- a/wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveAccelerationLimiter.hpp +++ b/wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveAccelerationLimiter.hpp @@ -23,8 +23,7 @@ namespace wpi::math { * Filters the provided voltages to limit a differential drive's linear and * angular acceleration. * - * The differential drive model can be created via the functions in - * LinearSystemId. + * The differential drive model can be created via the functions in Models. */ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter { public: diff --git a/wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveFeedforward.hpp b/wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveFeedforward.hpp index 1787da9bd5..0b320ad9fc 100644 --- a/wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveFeedforward.hpp +++ b/wpimath/src/main/native/include/wpi/math/controller/DifferentialDriveFeedforward.hpp @@ -6,7 +6,7 @@ #include "wpi/math/controller/DifferentialDriveWheelVoltages.hpp" #include "wpi/math/system/LinearSystem.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/angular_acceleration.hpp" #include "wpi/units/angular_velocity.hpp" @@ -42,7 +42,7 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward { decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, decltype(1_V / 1_rad_per_s) kVAngular, decltype(1_V / 1_rad_per_s_sq) kAAngular, wpi::units::meter_t trackwidth) - // See LinearSystemId::IdentifyDrivetrainSystem(decltype(1_V / 1_mps), + // See Models::DifferentialDriveFromSysId(decltype(1_V / 1_mps), // decltype(1_V / 1_mps_sq), decltype(1_V / 1_rad_per_s), decltype(1_V / // 1_rad_per_s_sq)) : DifferentialDriveFeedforward{kVLinear, kALinear, @@ -64,7 +64,7 @@ class WPILIB_DLLEXPORT DifferentialDriveFeedforward { decltype(1_V / 1_mps_sq) kALinear, decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) - : m_plant{wpi::math::LinearSystemId::IdentifyDrivetrainSystem( + : m_plant{wpi::math::Models::DifferentialDriveFromSysId( kVLinear, kALinear, kVAngular, kAAngular)}, m_kVLinear{kVLinear}, m_kALinear{kALinear}, diff --git a/wpimath/src/main/native/include/wpi/math/controller/ElevatorFeedforward.hpp b/wpimath/src/main/native/include/wpi/math/controller/ElevatorFeedforward.hpp index 263dbb3eaf..30a0e72c30 100644 --- a/wpimath/src/main/native/include/wpi/math/controller/ElevatorFeedforward.hpp +++ b/wpimath/src/main/native/include/wpi/math/controller/ElevatorFeedforward.hpp @@ -6,7 +6,6 @@ #include "wpi/math/controller/LinearPlantInversionFeedforward.hpp" #include "wpi/math/linalg/EigenCore.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" #include "wpi/math/util/MathShared.hpp" #include "wpi/units/length.hpp" #include "wpi/units/time.hpp" @@ -70,47 +69,6 @@ class ElevatorFeedforward { } } - /** - * Calculates the feedforward from the gains and setpoints assuming continuous - * control. - * - * @param velocity The velocity setpoint. - * @param acceleration The acceleration setpoint. - * @return The computed feedforward, in volts. - * @deprecated Use the current/next velocity overload instead. - */ - [[deprecated("Use the current/next velocity overload instead.")]] - constexpr wpi::units::volt_t Calculate( - wpi::units::unit_t velocity, - wpi::units::unit_t acceleration) const { - return kS * wpi::util::sgn(velocity) + kG + kV * velocity + - kA * acceleration; - } - - /** - * Calculates the feedforward from the gains and setpoints assuming continuous - * control. - * - * @param currentVelocity The current velocity setpoint. - * @param nextVelocity The next velocity setpoint. - * @param dt Time between velocity setpoints in seconds. - * @return The computed feedforward, in volts. - */ - [[deprecated("Use the current/next velocity overload instead.")]] - wpi::units::volt_t Calculate(wpi::units::unit_t currentVelocity, - wpi::units::unit_t nextVelocity, - wpi::units::second_t dt) const { - // See wpimath/algorithms.md#Elevator_feedforward for derivation - auto plant = LinearSystemId::IdentifyVelocitySystem(kV, kA); - LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt}; - - Vectord<1> r{{currentVelocity.value()}}; - Vectord<1> nextR{{nextVelocity.value()}}; - - return kG + kS * wpi::util::sgn(currentVelocity.value()) + - wpi::units::volt_t{feedforward.Calculate(r, nextR)(0)}; - } - /** * Calculates the feedforward from the gains and setpoint assuming discrete * control. Use this method when the setpoint does not change. diff --git a/wpimath/src/main/native/include/wpi/math/system/plant/DCMotor.hpp b/wpimath/src/main/native/include/wpi/math/system/DCMotor.hpp similarity index 98% rename from wpimath/src/main/native/include/wpi/math/system/plant/DCMotor.hpp rename to wpimath/src/main/native/include/wpi/math/system/DCMotor.hpp index b4b39ea784..c070bbfc59 100644 --- a/wpimath/src/main/native/include/wpi/math/system/plant/DCMotor.hpp +++ b/wpimath/src/main/native/include/wpi/math/system/DCMotor.hpp @@ -289,5 +289,5 @@ class WPILIB_DLLEXPORT DCMotor { } // namespace wpi::math -#include "wpi/math/system/plant/proto/DCMotorProto.hpp" -#include "wpi/math/system/plant/struct/DCMotorStruct.hpp" +#include "wpi/math/system/proto/DCMotorProto.hpp" +#include "wpi/math/system/struct/DCMotorStruct.hpp" diff --git a/wpimath/src/main/native/include/wpi/math/system/plant/LinearSystemId.hpp b/wpimath/src/main/native/include/wpi/math/system/Models.hpp similarity index 61% rename from wpimath/src/main/native/include/wpi/math/system/plant/LinearSystemId.hpp rename to wpimath/src/main/native/include/wpi/math/system/Models.hpp index 90def858f2..fc2b2b4578 100644 --- a/wpimath/src/main/native/include/wpi/math/system/plant/LinearSystemId.hpp +++ b/wpimath/src/main/native/include/wpi/math/system/Models.hpp @@ -4,13 +4,12 @@ #pragma once -#include #include #include +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/LinearSystem.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/angular_acceleration.hpp" #include "wpi/units/angular_velocity.hpp" @@ -22,9 +21,9 @@ namespace wpi::math { /** - * Linear system ID utility functions. + * Linear system factories. */ -class WPILIB_DLLEXPORT LinearSystemId { +class WPILIB_DLLEXPORT Models { public: template using Velocity_t = wpi::units::unit_t>>; /** - * Create a state-space model of the elevator system. The states of the system - * are [position, velocity]ᵀ, inputs are [voltage], and outputs are [position, - * velocity]ᵀ. + * Creates a flywheel state-space model from physical constants. + * + * The states are [angular velocity], the inputs are [voltage], and the + * outputs are [angular velocity]. + * + * @param motor The motor (or gearbox) attached to the flywheel. + * @param J The moment of inertia J of the flywheel. + * @param gearing Gear ratio from motor to flywheel (greater than 1 is a + * reduction). + * @throws std::domain_error if J <= 0 or gearing <= 0. + */ + static constexpr LinearSystem<1, 1, 1> FlywheelFromPhysicalConstants( + DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) { + if (J <= 0_kg_sq_m) { + throw std::domain_error("J must be greater than zero."); + } + if (gearing <= 0.0) { + throw std::domain_error("gearing must be greater than zero."); + } + + Matrixd<1, 1> A{ + {(-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)) + .value()}}; + Matrixd<1, 1> B{{(gearing * motor.Kt / (motor.R * J)).value()}}; + Matrixd<1, 1> C{{1.0}}; + Matrixd<1, 1> D{{0.0}}; + + return LinearSystem<1, 1, 1>(A, B, C, D); + } + + /** + * Creates a flywheel state-space model from SysId constants kᵥ (V/(rad/s)) + * and kₐ (V/(rad/s²)) from the feedforward model u = kᵥv + kₐa. + * + * The states are [velocity], the inputs are [voltage], and the outputs are + * [velocity]. + * + * @param kV The velocity gain, in V/(rad/s). + * @param kA The acceleration gain, in V/(rad/s²). + * @throws std::domain_error if kV < 0 or kA <= 0. + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid + */ + static constexpr LinearSystem<1, 1, 1> FlywheelFromSysId( + decltype(1_V / 1_rad_per_s) kV, decltype(1_V / 1_rad_per_s_sq) kA) { + if (kV < decltype(kV){0}) { + throw std::domain_error("Kv must be greater than or equal to zero."); + } + if (kA <= decltype(kA){0}) { + throw std::domain_error("Ka must be greater than zero."); + } + + Matrixd<1, 1> A{{-kV.value() / kA.value()}}; + Matrixd<1, 1> B{{1.0 / kA.value()}}; + Matrixd<1, 1> C{{1.0}}; + Matrixd<1, 1> D{{0.0}}; + + return LinearSystem<1, 1, 1>(A, B, C, D); + } + + /** + * Creates an elevator state-space model from physical constants. + * + * The states are [position, velocity], the inputs are [voltage], and the + * outputs are [position, velocity]. * * @param motor The motor (or gearbox) attached to the carriage. * @param mass The mass of the elevator carriage, in kilograms. * @param radius The radius of the elevator's driving drum, in meters. - * @param gearing Gear ratio from motor to carriage. + * @param gearing Gear ratio from motor to carriage (greater than 1 is a + * reduction). * @throws std::domain_error if mass <= 0, radius <= 0, or gearing <= 0. */ - static constexpr LinearSystem<2, 1, 2> ElevatorSystem( + static constexpr LinearSystem<2, 1, 2> ElevatorFromPhysicalConstants( DCMotor motor, wpi::units::kilogram_t mass, wpi::units::meter_t radius, double gearing) { if (mass <= 0_kg) { @@ -74,105 +136,21 @@ class WPILIB_DLLEXPORT LinearSystemId { } /** - * Create a state-space model of a single-jointed arm system.The states of the - * system are [angle, angular velocity]ᵀ, inputs are [voltage], and outputs - * are [angle, angular velocity]ᵀ. + * Creates an elevator state-space model from SysId constants kᵥ (V/(m/s)) and + * kₐ (V/(m/s²)) from the feedforward model u = kᵥv + kₐa. * - * @param motor The motor (or gearbox) attached to the arm. - * @param J The moment of inertia J of the arm. - * @param gearing Gear ratio from motor to arm. - * @throws std::domain_error if J <= 0 or gearing <= 0. - */ - static constexpr LinearSystem<2, 1, 2> SingleJointedArmSystem( - DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) { - if (J <= 0_kg_sq_m) { - throw std::domain_error("J must be greater than zero."); - } - if (gearing <= 0.0) { - throw std::domain_error("gearing must be greater than zero."); - } - - Matrixd<2, 2> A{ - {0.0, 1.0}, - {0.0, (-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)) - .value()}}; - Matrixd<2, 1> B{{0.0}, {(gearing * motor.Kt / (motor.R * J)).value()}}; - Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; - Matrixd<2, 1> D{{0.0}, {0.0}}; - - return LinearSystem<2, 1, 2>(A, B, C, D); - } - - /** - * Create a state-space model for a 1 DOF velocity system from its kV - * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be - * found using SysId. The states of the system are [velocity], inputs are - * [voltage], and outputs are [velocity]. + * The states are [position, velocity], the inputs are [voltage], and the + * outputs are [position, velocity]. * - * You MUST use an SI unit (i.e. meters or radians) for the Distance template - * argument. You may still use non-SI units (such as feet or inches) for the - * actual method arguments; they will automatically be converted to SI - * internally. - * - * The parameters provided by the user are from this feedforward model: - * - * u = K_v v + K_a a - * - * @param kV The velocity gain, in volts/(unit/sec). - * @param kA The acceleration gain, in volts/(unit/sec²). - * @throws std::domain_error if kV < 0 or kA <= 0. - * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid - */ - template - requires std::same_as || - std::same_as - static constexpr LinearSystem<1, 1, 1> IdentifyVelocitySystem( - decltype(1_V / Velocity_t(1)) kV, - decltype(1_V / Acceleration_t(1)) kA) { - if (kV < decltype(kV){0}) { - throw std::domain_error("Kv must be greater than or equal to zero."); - } - if (kA <= decltype(kA){0}) { - throw std::domain_error("Ka must be greater than zero."); - } - - Matrixd<1, 1> A{{-kV.value() / kA.value()}}; - Matrixd<1, 1> B{{1.0 / kA.value()}}; - Matrixd<1, 1> C{{1.0}}; - Matrixd<1, 1> D{{0.0}}; - - return LinearSystem<1, 1, 1>(A, B, C, D); - } - - /** - * Create a state-space model for a 1 DOF position system from its kV - * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be - * found using SysId. the states of the system are [position, velocity]ᵀ, - * inputs are [voltage], and outputs are [position, velocity]ᵀ. - * - * You MUST use an SI unit (i.e. meters or radians) for the Distance template - * argument. You may still use non-SI units (such as feet or inches) for the - * actual method arguments; they will automatically be converted to SI - * internally. - * - * The parameters provided by the user are from this feedforward model: - * - * u = K_v v + K_a a - * - * @param kV The velocity gain, in volts/(unit/sec). - * @param kA The acceleration gain, in volts/(unit/sec²). + * @param kV The velocity gain, in V/(m/s). + * @param kA The acceleration gain, in V/(m/s²). * * @throws std::domain_error if kV < 0 or kA <= 0. * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ - template - requires std::same_as || - std::same_as - static constexpr LinearSystem<2, 1, 2> IdentifyPositionSystem( - decltype(1_V / Velocity_t(1)) kV, - decltype(1_V / Acceleration_t(1)) kA) { + static constexpr LinearSystem<2, 1, 2> ElevatorFromSysId( + decltype(1_V / 1_mps) kV, decltype(1_V / 1_mps_sq) kA) { if (kV < decltype(kV){0}) { throw std::domain_error("Kv must be greater than or equal to zero."); } @@ -189,167 +167,18 @@ class WPILIB_DLLEXPORT LinearSystemId { } /** - * Identify a differential drive drivetrain given the drivetrain's kV and kA - * in both linear {(volts/(meter/sec), (volts/(meter/sec²))} and angular - * {(volts/(radian/sec), (volts/(radian/sec²))} cases. These constants can be - * found using SysId. + * Create a single-jointed arm state-space model from physical constants. * - * States: [[left velocity], [right velocity]]
- * Inputs: [[left voltage], [right voltage]]
- * Outputs: [[left velocity], [right velocity]] + * The states are [angle, angular velocity], the inputs are [voltage], and the + * outputs are [angle, angular velocity]. * - * @param kVLinear The linear velocity gain in volts per (meters per second). - * @param kALinear The linear acceleration gain in volts per (meters per - * second squared). - * @param kVAngular The angular velocity gain in volts per (meters per - * second). - * @param kAAngular The angular acceleration gain in volts per (meters per - * second squared). - * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, - * or kAAngular <= 0. - * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid - */ - static constexpr LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( - decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, - decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) { - if (kVLinear <= decltype(kVLinear){0}) { - throw std::domain_error("Kv,linear must be greater than zero."); - } - if (kALinear <= decltype(kALinear){0}) { - throw std::domain_error("Ka,linear must be greater than zero."); - } - if (kVAngular <= decltype(kVAngular){0}) { - throw std::domain_error("Kv,angular must be greater than zero."); - } - if (kAAngular <= decltype(kAAngular){0}) { - throw std::domain_error("Ka,angular must be greater than zero."); - } - - double A1 = -(kVLinear.value() / kALinear.value() + - kVAngular.value() / kAAngular.value()); - double A2 = -(kVLinear.value() / kALinear.value() - - kVAngular.value() / kAAngular.value()); - double B1 = 1.0 / kALinear.value() + 1.0 / kAAngular.value(); - double B2 = 1.0 / kALinear.value() - 1.0 / kAAngular.value(); - - A1 /= 2.0; - A2 /= 2.0; - B1 /= 2.0; - B2 /= 2.0; - - Matrixd<2, 2> A{{A1, A2}, {A2, A1}}; - Matrixd<2, 2> B{{B1, B2}, {B2, B1}}; - Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; - Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; - - return LinearSystem<2, 2, 2>(A, B, C, D); - } - - /** - * Identify a differential drive drivetrain given the drivetrain's kV and kA - * in both linear {(volts/(meter/sec)), (volts/(meter/sec²))} and angular - * {(volts/(radian/sec)), (volts/(radian/sec²))} cases. This can be found - * using SysId. - * - * States: [[left velocity], [right velocity]]
- * Inputs: [[left voltage], [right voltage]]
- * Outputs: [[left velocity], [right velocity]] - * - * @param kVLinear The linear velocity gain in volts per (meters per - * second). - * @param kALinear The linear acceleration gain in volts per (meters per - * second squared). - * @param kVAngular The angular velocity gain in volts per (radians per - * second). - * @param kAAngular The angular acceleration gain in volts per (radians per - * second squared). - * @param trackwidth The distance between the differential drive's left and - * right wheels, in meters. - * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, - * kAAngular <= 0, or trackwidth <= 0. - * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid - */ - static constexpr LinearSystem<2, 2, 2> IdentifyDrivetrainSystem( - decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, - decltype(1_V / 1_rad_per_s) kVAngular, - decltype(1_V / 1_rad_per_s_sq) kAAngular, - wpi::units::meter_t trackwidth) { - if (kVLinear <= decltype(kVLinear){0}) { - throw std::domain_error("Kv,linear must be greater than zero."); - } - if (kALinear <= decltype(kALinear){0}) { - throw std::domain_error("Ka,linear must be greater than zero."); - } - if (kVAngular <= decltype(kVAngular){0}) { - throw std::domain_error("Kv,angular must be greater than zero."); - } - if (kAAngular <= decltype(kAAngular){0}) { - throw std::domain_error("Ka,angular must be greater than zero."); - } - if (trackwidth <= 0_m) { - throw std::domain_error("r must be greater than zero."); - } - - // We want to find a factor to include in Kv,angular that will convert - // `u = Kv,angular omega` to `u = Kv,angular v`. - // - // v = omega r - // omega = v/r - // omega = 1/r v - // omega = 1/(trackwidth/2) v - // omega = 2/trackwidth v - // - // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s) - // to V/(m/s). - return IdentifyDrivetrainSystem(kVLinear, kALinear, - kVAngular * 2.0 / trackwidth * 1_rad, - kAAngular * 2.0 / trackwidth * 1_rad); - } - - /** - * Create a state-space model of a flywheel system, the states of the system - * are [angular velocity], inputs are [voltage], and outputs are [angular - * velocity]. - * - * @param motor The motor (or gearbox) attached to the flywheel. - * @param J The moment of inertia J of the flywheel. - * @param gearing Gear ratio from motor to flywheel. + * @param motor The motor (or gearbox) attached to the arm. + * @param J The moment of inertia J of the arm. + * @param gearing Gear ratio from motor to arm (greater than 1 is a + * reduction). * @throws std::domain_error if J <= 0 or gearing <= 0. */ - static constexpr LinearSystem<1, 1, 1> FlywheelSystem( - DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) { - if (J <= 0_kg_sq_m) { - throw std::domain_error("J must be greater than zero."); - } - if (gearing <= 0.0) { - throw std::domain_error("gearing must be greater than zero."); - } - - Matrixd<1, 1> A{ - {(-gcem::pow(gearing, 2) * motor.Kt / (motor.Kv * motor.R * J)) - .value()}}; - Matrixd<1, 1> B{{(gearing * motor.Kt / (motor.R * J)).value()}}; - Matrixd<1, 1> C{{1.0}}; - Matrixd<1, 1> D{{0.0}}; - - return LinearSystem<1, 1, 1>(A, B, C, D); - } - - /** - * Create a state-space model of a DC motor system. The states of the system - * are [angular position, angular velocity]ᵀ, inputs are [voltage], and - * outputs are [angular position, angular velocity]ᵀ. - * - * @param motor The motor (or gearbox) attached to the system. - * @param J the moment of inertia J of the DC motor. - * @param gearing Gear ratio from motor to output. - * @throws std::domain_error if J <= 0 or gearing <= 0. - * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid - */ - static constexpr LinearSystem<2, 1, 2> DCMotorSystem( + static constexpr LinearSystem<2, 1, 2> SingleJointedArmFromPhysicalConstants( DCMotor motor, wpi::units::kilogram_square_meter_t J, double gearing) { if (J <= 0_kg_sq_m) { throw std::domain_error("J must be greater than zero."); @@ -370,32 +199,21 @@ class WPILIB_DLLEXPORT LinearSystemId { } /** - * Create a state-space model of a DC motor system from its kV - * (volts/(unit/sec)) and kA (volts/(unit/sec²)). These constants can be - * found using SysId. the states of the system are [angular position, angular - * velocity]ᵀ, inputs are [voltage], and outputs are [angular position, - * angular velocity]ᵀ. + * Creates a single-jointed arm state-space model from SysId constants kᵥ + * (V/(rad/s)) and kₐ (V/(rad/s²)) from the feedforward model u = kᵥv + kₐa. * - * You MUST use an SI unit (i.e. meters or radians) for the Distance template - * argument. You may still use non-SI units (such as feet or inches) for the - * actual method arguments; they will automatically be converted to SI - * internally. - * - * The parameters provided by the user are from this feedforward model: - * - * u = K_v v + K_a a + * The states are [position, velocity], the inputs are [voltage], and the + * outputs are [position, velocity]. * * @param kV The velocity gain, in volts/(unit/sec). * @param kA The acceleration gain, in volts/(unit/sec²). * * @throws std::domain_error if kV < 0 or kA <= 0. + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid */ - template - requires std::same_as || - std::same_as - static constexpr LinearSystem<2, 1, 2> DCMotorSystem( - decltype(1_V / Velocity_t(1)) kV, - decltype(1_V / Acceleration_t(1)) kA) { + static constexpr LinearSystem<2, 1, 2> SingleJointedArmFromSysId( + decltype(1_V / 1_rad_per_s) kV, decltype(1_V / 1_rad_per_s_sq) kA) { if (kV < decltype(kV){0}) { throw std::domain_error("Kv must be greater than or equal to zero."); } @@ -404,7 +222,7 @@ class WPILIB_DLLEXPORT LinearSystemId { } Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}}; - Matrixd<2, 1> B{0.0, 1.0 / kA.value()}; + Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}}; Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; Matrixd<2, 1> D{{0.0}, {0.0}}; @@ -412,21 +230,23 @@ class WPILIB_DLLEXPORT LinearSystemId { } /** - * Create a state-space model of differential drive drivetrain. In this model, - * the states are [left velocity, right velocity]ᵀ, the inputs are [left + * Creates a differential drive state-space model from physical constants. + * + * The states are [left velocity, right velocity], the inputs are [left * voltage, right voltage], and the outputs are [left velocity, right - * velocity]ᵀ. + * velocity]. * * @param motor The motor (or gearbox) driving the drivetrain. * @param mass The mass of the robot in kilograms. * @param r The radius of the wheels in meters. * @param rb The radius of the base (half of the trackwidth), in meters. * @param J The moment of inertia of the robot. - * @param gearing Gear ratio from motor to wheel. + * @param gearing Gear ratio from motor to wheel (greater than 1 is a + * reduction). * @throws std::domain_error if mass <= 0, r <= 0, rb <= 0, J <= 0, or * gearing <= 0. */ - static constexpr LinearSystem<2, 2, 2> DrivetrainVelocitySystem( + static constexpr LinearSystem<2, 2, 2> DifferentialDriveFromPhysicalConstants( const DCMotor& motor, wpi::units::kilogram_t mass, wpi::units::meter_t r, wpi::units::meter_t rb, wpi::units::kilogram_square_meter_t J, double gearing) { @@ -465,6 +285,119 @@ class WPILIB_DLLEXPORT LinearSystemId { return LinearSystem<2, 2, 2>(A, B, C, D); } + + /** + * Creates a differential drive state-space model from SysId constants kᵥ and + * kₐ in both linear {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s), + * (V/(rad/s²))} cases. + * + * The states are [left velocity, right velocity], the inputs are [left + * voltage, right voltage], and the outputs are [left velocity, right + * velocity]. + * + * @param kVLinear The linear velocity gain in volts per (meters per second). + * @param kALinear The linear acceleration gain in volts per (meters per + * second squared). + * @param kVAngular The angular velocity gain in volts per (meters per + * second). + * @param kAAngular The angular acceleration gain in volts per (meters per + * second squared). + * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, + * or kAAngular <= 0. + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid + */ + static constexpr LinearSystem<2, 2, 2> DifferentialDriveFromSysId( + decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, + decltype(1_V / 1_mps) kVAngular, decltype(1_V / 1_mps_sq) kAAngular) { + if (kVLinear <= decltype(kVLinear){0}) { + throw std::domain_error("Kv,linear must be greater than zero."); + } + if (kALinear <= decltype(kALinear){0}) { + throw std::domain_error("Ka,linear must be greater than zero."); + } + if (kVAngular <= decltype(kVAngular){0}) { + throw std::domain_error("Kv,angular must be greater than zero."); + } + if (kAAngular <= decltype(kAAngular){0}) { + throw std::domain_error("Ka,angular must be greater than zero."); + } + + double A1 = -0.5 * (kVLinear.value() / kALinear.value() + + kVAngular.value() / kAAngular.value()); + double A2 = -0.5 * (kVLinear.value() / kALinear.value() - + kVAngular.value() / kAAngular.value()); + double B1 = 0.5 / kALinear.value() + 0.5 / kAAngular.value(); + double B2 = 0.5 / kALinear.value() - 0.5 / kAAngular.value(); + + Matrixd<2, 2> A{{A1, A2}, {A2, A1}}; + Matrixd<2, 2> B{{B1, B2}, {B2, B1}}; + Matrixd<2, 2> C{{1.0, 0.0}, {0.0, 1.0}}; + Matrixd<2, 2> D{{0.0, 0.0}, {0.0, 0.0}}; + + return LinearSystem<2, 2, 2>(A, B, C, D); + } + + /** + * Creates a differential drive state-space model from SysId constants kᵥ and + * kₐ in both linear {(V/(m/s), (V/(m/s²))} and angular {(V/(rad/s), + * (V/(rad/s²))} cases. + * + * The states are [left velocity, right velocity], the inputs are [left + * voltage, right voltage], and the outputs are [left velocity, right + * velocity]. + * + * @param kVLinear The linear velocity gain in volts per (meters per + * second). + * @param kALinear The linear acceleration gain in volts per (meters per + * second squared). + * @param kVAngular The angular velocity gain in volts per (radians per + * second). + * @param kAAngular The angular acceleration gain in volts per (radians per + * second squared). + * @param trackwidth The distance between the differential drive's left and + * right wheels, in meters. + * @throws domain_error if kVLinear <= 0, kALinear <= 0, kVAngular <= 0, + * kAAngular <= 0, or trackwidth <= 0. + * @see https://github.com/wpilibsuite/allwpilib/tree/main/sysid + */ + static constexpr LinearSystem<2, 2, 2> DifferentialDriveFromSysId( + decltype(1_V / 1_mps) kVLinear, decltype(1_V / 1_mps_sq) kALinear, + decltype(1_V / 1_rad_per_s) kVAngular, + decltype(1_V / 1_rad_per_s_sq) kAAngular, + wpi::units::meter_t trackwidth) { + if (kVLinear <= decltype(kVLinear){0}) { + throw std::domain_error("Kv,linear must be greater than zero."); + } + if (kALinear <= decltype(kALinear){0}) { + throw std::domain_error("Ka,linear must be greater than zero."); + } + if (kVAngular <= decltype(kVAngular){0}) { + throw std::domain_error("Kv,angular must be greater than zero."); + } + if (kAAngular <= decltype(kAAngular){0}) { + throw std::domain_error("Ka,angular must be greater than zero."); + } + if (trackwidth <= 0_m) { + throw std::domain_error("r must be greater than zero."); + } + + // We want to find a factor to include in Kv,angular that will convert + // `u = Kv,angular omega` to `u = Kv,angular v`. + // + // v = omega r + // omega = v/r + // omega = 1/r v + // omega = 1/(trackwidth/2) v + // omega = 2/trackwidth v + // + // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s) + // to V/(m/s). + return DifferentialDriveFromSysId(kVLinear, kALinear, + kVAngular * 2.0 / trackwidth * 1_rad, + kAAngular * 2.0 / trackwidth * 1_rad); + } }; } // namespace wpi::math diff --git a/wpimath/src/main/native/include/wpi/math/system/plant/proto/DCMotorProto.hpp b/wpimath/src/main/native/include/wpi/math/system/proto/DCMotorProto.hpp similarity index 89% rename from wpimath/src/main/native/include/wpi/math/system/plant/proto/DCMotorProto.hpp rename to wpimath/src/main/native/include/wpi/math/system/proto/DCMotorProto.hpp index 658e08502a..30c7d41a7b 100644 --- a/wpimath/src/main/native/include/wpi/math/system/plant/proto/DCMotorProto.hpp +++ b/wpimath/src/main/native/include/wpi/math/system/proto/DCMotorProto.hpp @@ -4,10 +4,10 @@ #pragma once -#include "wpi/math/system/plant/DCMotor.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/protobuf/Protobuf.hpp" -#include "wpimath/protobuf/plant.npb.h" +#include "wpimath/protobuf/system.npb.h" template <> struct WPILIB_DLLEXPORT wpi::util::Protobuf { diff --git a/wpimath/src/main/native/include/wpi/math/system/plant/struct/DCMotorStruct.hpp b/wpimath/src/main/native/include/wpi/math/system/struct/DCMotorStruct.hpp similarity index 95% rename from wpimath/src/main/native/include/wpi/math/system/plant/struct/DCMotorStruct.hpp rename to wpimath/src/main/native/include/wpi/math/system/struct/DCMotorStruct.hpp index 9a502dab81..c46fd1c063 100644 --- a/wpimath/src/main/native/include/wpi/math/system/plant/struct/DCMotorStruct.hpp +++ b/wpimath/src/main/native/include/wpi/math/system/struct/DCMotorStruct.hpp @@ -4,7 +4,7 @@ #pragma once -#include "wpi/math/system/plant/DCMotor.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/util/SymbolExports.hpp" #include "wpi/util/struct/Struct.hpp" diff --git a/wpimath/src/main/proto/plant.proto b/wpimath/src/main/proto/plant.proto deleted file mode 100644 index db11c398e1..0000000000 --- a/wpimath/src/main/proto/plant.proto +++ /dev/null @@ -1,13 +0,0 @@ -syntax = "proto3"; - -package wpi.proto; - -option java_package = "org.wpilib.math.proto"; - -message ProtobufDCMotor { - double nominal_voltage = 1; - double stall_torque = 2; - double stall_current = 3; - double free_current = 4; - double free_speed = 5; -} diff --git a/wpimath/src/main/proto/system.proto b/wpimath/src/main/proto/system.proto index 8cdb6a20f6..a28ba936a4 100644 --- a/wpimath/src/main/proto/system.proto +++ b/wpimath/src/main/proto/system.proto @@ -6,6 +6,14 @@ import "wpimath.proto"; option java_package = "org.wpilib.math.proto"; +message ProtobufDCMotor { + double nominal_voltage = 1; + double stall_torque = 2; + double stall_current = 3; + double free_current = 4; + double free_speed = 5; +} + message ProtobufLinearSystem { uint32 num_states = 1; uint32 num_inputs = 2; diff --git a/wpimath/src/main/python/pyproject.toml b/wpimath/src/main/python/pyproject.toml index 5d2f7615a9..b905cb09b0 100644 --- a/wpimath/src/main/python/pyproject.toml +++ b/wpimath/src/main/python/pyproject.toml @@ -1560,17 +1560,15 @@ SplineHelper = "wpi/math/spline/SplineHelper.hpp" SplineParameterizer = "wpi/math/spline/SplineParameterizer.hpp" # wpi/math/system +DCMotor = "wpi/math/system/DCMotor.hpp" # Discretization = "wpi/math/system/Discretization.hpp" LinearSystem = "wpi/math/system/LinearSystem.hpp" LinearSystemLoop = "wpi/math/system/LinearSystemLoop.hpp" # LinearSystemUtil = "wpi/math/system/LinearSystemUtil.hpp" +Models = "wpi/math/system/Models.hpp" # NumericalIntegration = "wpi/math/system/NumericalIntegration.hpp" # NumericalJacobian = "wpi/math/system/NumericalJacobian.hpp" -# wpi/math/system/plant -DCMotor = "wpi/math/system/plant/DCMotor.hpp" -LinearSystemId = "wpi/math/system/plant/LinearSystemId.hpp" - # wpi/math/trajectory ExponentialProfile = "wpi/math/trajectory/ExponentialProfile.hpp" Trajectory = "wpi/math/trajectory/Trajectory.hpp" diff --git a/wpimath/src/main/python/semiwrap/ElevatorFeedforward.yml b/wpimath/src/main/python/semiwrap/ElevatorFeedforward.yml index ff7d5a10d5..d98aa51c03 100644 --- a/wpimath/src/main/python/semiwrap/ElevatorFeedforward.yml +++ b/wpimath/src/main/python/semiwrap/ElevatorFeedforward.yml @@ -10,10 +10,6 @@ classes: wpi::units::volt_t, wpi::units::volt_t, wpi::units::unit_t, wpi::units::unit_t: Calculate: overloads: - wpi::units::unit_t, wpi::units::unit_t [const]: - ignore: true - wpi::units::unit_t, wpi::units::unit_t, wpi::units::second_t [const]: - ignore: true wpi::units::unit_t [const]: wpi::units::unit_t, wpi::units::unit_t [const]: MaxAchievableVelocity: diff --git a/wpimath/src/main/python/semiwrap/LinearSystemId.yml b/wpimath/src/main/python/semiwrap/LinearSystemId.yml deleted file mode 100644 index dd06745334..0000000000 --- a/wpimath/src/main/python/semiwrap/LinearSystemId.yml +++ /dev/null @@ -1,59 +0,0 @@ -classes: - wpi::math::LinearSystemId: - typealias: - - kv_meters = wpi::units::unit_t>> - - kv_radians = wpi::units::unit_t>> - - ka_meters = wpi::units::unit_t>> - - ka_radians = wpi::units::unit_t>> - methods: - ElevatorSystem: - SingleJointedArmSystem: - IdentifyVelocitySystem: - rename: identifyVelocitySystemMeters - cpp_code: | - [](kv_meters kV, ka_meters kA) { - return wpi::math::LinearSystemId::IdentifyVelocitySystem(kV, kA); - } - IdentifyPositionSystem: - rename: identifyPositionSystemMeters - cpp_code: | - [](kv_meters kV, ka_meters kA) { - return wpi::math::LinearSystemId::IdentifyPositionSystem(kV, kA); - } - IdentifyDrivetrainSystem: - overloads: - decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_mps), decltype(1_V/1_mps_sq): - cpp_code: | - [](kv_meters kVlinear, ka_meters kAlinear, kv_meters kVangular, ka_meters kAangular) { - return wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVlinear, kAlinear, kVangular, kAangular); - } - decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_rad_per_s), decltype(1_V/1_rad_per_s_sq), wpi::units::meter_t: - cpp_code: | - [](kv_meters kVlinear, ka_meters kAlinear, kv_radians kVangular, ka_radians kAangular, wpi::units::meter_t trackWidth) { - return wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVlinear, kAlinear, kVangular, kAangular, trackWidth); - } - FlywheelSystem: - DCMotorSystem: - overloads: - DCMotor, wpi::units::kilogram_square_meter_t, double: - decltype(1_V/Velocity_t (1)), decltype(1_V/Acceleration_t (1)): - cpp_code: | - [](kv_meters kv, ka_meters ka) { - return wpi::math::LinearSystemId::DCMotorSystem(kv, ka); - } - DrivetrainVelocitySystem: - - inline_code: | - .def_static("DCMotorSystemRadians", [](kv_radians kV, ka_radians kA) { - return wpi::math::LinearSystemId::DCMotorSystem(kV, kA); - }, py::arg("kV"), py::arg("kA"), release_gil() - ) - - .def_static("identifyVelocitySystemRadians", [](kv_radians kV, ka_radians kA) { - return wpi::math::LinearSystemId::IdentifyVelocitySystem(kV, kA); - }, py::arg("kV"), py::arg("kA"), release_gil() - ) - .def_static("identifyPositionSystemRadians", [](kv_radians kV, ka_radians kA) { - return wpi::math::LinearSystemId::IdentifyPositionSystem(kV, kA); - }, py::arg("kV"), py::arg("kA"), release_gil() - ) diff --git a/wpimath/src/main/python/semiwrap/Models.yml b/wpimath/src/main/python/semiwrap/Models.yml new file mode 100644 index 0000000000..760cc7367f --- /dev/null +++ b/wpimath/src/main/python/semiwrap/Models.yml @@ -0,0 +1,14 @@ +classes: + wpi::math::Models: + methods: + FlywheelFromPhysicalConstants: + FlywheelFromSysId: + ElevatorFromPhysicalConstants: + ElevatorFromSysId: + SingleJointedArmFromPhysicalConstants: + SingleJointedArmFromSysId: + DifferentialDriveFromPhysicalConstants: + DifferentialDriveFromSysId: + overloads: + decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_mps), decltype(1_V/1_mps_sq): + decltype(1_V/1_mps), decltype(1_V/1_mps_sq), decltype(1_V/1_rad_per_s), decltype(1_V/1_rad_per_s_sq), wpi::units::meter_t: diff --git a/wpimath/src/main/python/wpimath/__init__.py b/wpimath/src/main/python/wpimath/__init__.py index da2a921c7f..3e3d27b222 100644 --- a/wpimath/src/main/python/wpimath/__init__.py +++ b/wpimath/src/main/python/wpimath/__init__.py @@ -60,7 +60,6 @@ from ._wpimath import ( LinearQuadraticRegulator_2_1, LinearQuadraticRegulator_2_2, LinearQuadraticRegulator_3_2, - LinearSystemId, LinearSystemLoop_1_1_1, LinearSystemLoop_2_1_1, LinearSystemLoop_2_1_2, @@ -94,6 +93,7 @@ from ._wpimath import ( MecanumDriveWheelPositions, MecanumDriveWheelSpeeds, MedianFilter, + Models, PIDController, Pose2d, Pose3d, @@ -247,7 +247,6 @@ __all__ = [ "LinearQuadraticRegulator_2_1", "LinearQuadraticRegulator_2_2", "LinearQuadraticRegulator_3_2", - "LinearSystemId", "LinearSystemLoop_1_1_1", "LinearSystemLoop_2_1_1", "LinearSystemLoop_2_1_2", @@ -281,6 +280,7 @@ __all__ = [ "MecanumDriveWheelPositions", "MecanumDriveWheelSpeeds", "MedianFilter", + "Models", "PIDController", "Pose2d", "Pose3d", diff --git a/wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveAccelerationLimiterTest.java b/wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveAccelerationLimiterTest.java index fc120cff91..4006a09876 100644 --- a/wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveAccelerationLimiterTest.java +++ b/wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveAccelerationLimiterTest.java @@ -14,7 +14,7 @@ import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; class DifferentialDriveAccelerationLimiterTest { @Test @@ -24,7 +24,7 @@ class DifferentialDriveAccelerationLimiterTest { final double maxA = 2.0; final double maxAlpha = 2.0; - var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0); var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, maxA, maxAlpha); @@ -114,7 +114,7 @@ class DifferentialDriveAccelerationLimiterTest { final double trackwidth = 0.9; final double dt = 0.005; - var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0); // Limits are so high, they don't get hit, so states of constrained and // unconstrained systems should match @@ -178,7 +178,7 @@ class DifferentialDriveAccelerationLimiterTest { final double maxA = 2.0; final double maxAlpha = 2.0; - var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0); var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha); @@ -237,7 +237,7 @@ class DifferentialDriveAccelerationLimiterTest { @Test void testMinAccelGreaterThanMaxAccel() { - var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0); assertDoesNotThrow(() -> new DifferentialDriveAccelerationLimiter(plant, 1, -1, 1, 1e3)); assertThrows( IllegalArgumentException.class, diff --git a/wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveFeedforwardTest.java b/wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveFeedforwardTest.java index 130d8bc88d..862c0264b5 100644 --- a/wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveFeedforwardTest.java +++ b/wpimath/src/test/java/org/wpilib/math/controller/DifferentialDriveFeedforwardTest.java @@ -12,7 +12,7 @@ import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; import org.wpilib.math.system.LinearSystem; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; class DifferentialDriveFeedforwardTest { private static final double kVLinear = 1.0; @@ -27,8 +27,7 @@ class DifferentialDriveFeedforwardTest { DifferentialDriveFeedforward differentialDriveFeedforward = new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular, trackwidth); LinearSystem plant = - LinearSystemId.identifyDrivetrainSystem( - kVLinear, kALinear, kVAngular, kAAngular, trackwidth); + Models.differentialDriveFromSysId(kVLinear, kALinear, kVAngular, kAAngular, trackwidth); for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) { for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) { for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) { @@ -58,7 +57,7 @@ class DifferentialDriveFeedforwardTest { DifferentialDriveFeedforward differentialDriveFeedforward = new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular); LinearSystem plant = - LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular); + Models.differentialDriveFromSysId(kVLinear, kALinear, kVAngular, kAAngular); for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) { for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) { for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) { diff --git a/wpimath/src/test/java/org/wpilib/math/controller/ImplicitModelFollowerTest.java b/wpimath/src/test/java/org/wpilib/math/controller/ImplicitModelFollowerTest.java index 7775b1edeb..404e81f70a 100644 --- a/wpimath/src/test/java/org/wpilib/math/controller/ImplicitModelFollowerTest.java +++ b/wpimath/src/test/java/org/wpilib/math/controller/ImplicitModelFollowerTest.java @@ -12,14 +12,14 @@ import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; class ImplicitModelFollowerTest { @Test void testSameModel() { final double dt = 0.005; - var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0); var imf = new ImplicitModelFollower<>(plant, plant); @@ -61,10 +61,10 @@ class ImplicitModelFollowerTest { void testSlowerRefModel() { final double dt = 0.005; - var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0); + var plant = Models.differentialDriveFromSysId(1.0, 1.0, 1.0, 1.0); // Linear acceleration is slower, but angular acceleration is the same - var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0); + var plantRef = Models.differentialDriveFromSysId(1.0, 2.0, 1.0, 1.0); var imf = new ImplicitModelFollower<>(plant, plantRef); diff --git a/wpimath/src/test/java/org/wpilib/math/controller/LTVDifferentialDriveControllerTest.java b/wpimath/src/test/java/org/wpilib/math/controller/LTVDifferentialDriveControllerTest.java index c8d22cadf5..1282b0db59 100644 --- a/wpimath/src/test/java/org/wpilib/math/controller/LTVDifferentialDriveControllerTest.java +++ b/wpimath/src/test/java/org/wpilib/math/controller/LTVDifferentialDriveControllerTest.java @@ -18,8 +18,8 @@ import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; import org.wpilib.math.numbers.N5; import org.wpilib.math.system.LinearSystem; +import org.wpilib.math.system.Models; import org.wpilib.math.system.NumericalIntegration; -import org.wpilib.math.system.plant.LinearSystemId; import org.wpilib.math.trajectory.TrajectoryConfig; import org.wpilib.math.trajectory.TrajectoryGenerator; import org.wpilib.math.util.MathUtil; @@ -52,7 +52,7 @@ class LTVDifferentialDriveControllerTest { private static final double kAngularV = 1.382; // V/(m/s) private static final double kAngularA = 0.08495; // V/(m/s²) private static final LinearSystem plant = - LinearSystemId.identifyDrivetrainSystem(kLinearV, kLinearA, kAngularV, kAngularA); + Models.differentialDriveFromSysId(kLinearV, kLinearA, kAngularV, kAngularA); private static final double kTrackwidth = 0.9; private static Matrix dynamics(Matrix x, Matrix u) { diff --git a/wpimath/src/test/java/org/wpilib/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/org/wpilib/math/controller/LinearQuadraticRegulatorTest.java index 94226f735e..0a6c730b1b 100644 --- a/wpimath/src/test/java/org/wpilib/math/controller/LinearQuadraticRegulatorTest.java +++ b/wpimath/src/test/java/org/wpilib/math/controller/LinearQuadraticRegulatorTest.java @@ -10,9 +10,9 @@ import org.junit.jupiter.api.Test; import org.wpilib.math.linalg.MatBuilder; import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.Discretization; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.math.util.Nat; import org.wpilib.math.util.Num; @@ -25,7 +25,7 @@ class LinearQuadraticRegulatorTest { var r = 0.0181864; var G = 1.0; - var plant = LinearSystemId.createElevatorSystem(motors, m, r, G); + var plant = Models.elevatorFromPhysicalConstants(motors, m, r, G); var qElms = VecBuilder.fill(0.02, 0.4); var rElms = VecBuilder.fill(12.0); @@ -42,7 +42,7 @@ class LinearQuadraticRegulatorTest { var dt = 0.020; var plant = - LinearSystemId.createElevatorSystem( + Models.elevatorFromPhysicalConstants( DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67); var K = @@ -61,7 +61,7 @@ class LinearQuadraticRegulatorTest { var r = 0.4; var G = 100.0; - var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G); + var plant = Models.singleJointedArmFromPhysicalConstants(motors, 1d / 3d * m * r * r, G); var qElms = VecBuilder.fill(0.01745, 0.08726); var rElms = VecBuilder.fill(12.0); @@ -160,7 +160,7 @@ class LinearQuadraticRegulatorTest { var dt = 0.02; var plant = - LinearSystemId.createElevatorSystem( + Models.elevatorFromPhysicalConstants( DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67); var regulator = diff --git a/wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java index 14523a5438..f9ab499cb4 100644 --- a/wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java +++ b/wpimath/src/test/java/org/wpilib/math/controller/LinearSystemLoopTest.java @@ -14,10 +14,10 @@ import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; import org.wpilib.math.system.LinearSystemLoop; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.math.trajectory.TrapezoidProfile; import org.wpilib.math.util.Nat; @@ -27,7 +27,7 @@ class LinearSystemLoopTest { private static final Random random = new Random(); LinearSystem m_plant = - LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0); + Models.elevatorFromPhysicalConstants(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0); @SuppressWarnings("unchecked") KalmanFilter m_observer = @@ -95,7 +95,7 @@ class LinearSystemLoopTest { @Test void testFlywheelEnabled() { LinearSystem plant = - LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0); + Models.flywheelFromPhysicalConstants(DCMotor.getNEO(2), 0.00289, 1.0); KalmanFilter observer = new KalmanFilter<>( Nat.N1(), Nat.N1(), plant, VecBuilder.fill(1.0), VecBuilder.fill(kPositionStddev), kDt); diff --git a/wpimath/src/test/java/org/wpilib/math/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/org/wpilib/math/estimator/ExtendedKalmanFilterTest.java index 1a3e551b46..fa2ad9ae92 100644 --- a/wpimath/src/test/java/org/wpilib/math/estimator/ExtendedKalmanFilterTest.java +++ b/wpimath/src/test/java/org/wpilib/math/estimator/ExtendedKalmanFilterTest.java @@ -18,9 +18,9 @@ import org.wpilib.math.numbers.N2; import org.wpilib.math.numbers.N3; import org.wpilib.math.numbers.N5; import org.wpilib.math.random.Normal; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.NumericalIntegration; import org.wpilib.math.system.NumericalJacobian; -import org.wpilib.math.system.plant.DCMotor; import org.wpilib.math.trajectory.TrajectoryConfig; import org.wpilib.math.trajectory.TrajectoryGenerator; import org.wpilib.math.util.Nat; diff --git a/wpimath/src/test/java/org/wpilib/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/org/wpilib/math/estimator/KalmanFilterTest.java index d76ed156f0..96a9875d3f 100644 --- a/wpimath/src/test/java/org/wpilib/math/estimator/KalmanFilterTest.java +++ b/wpimath/src/test/java/org/wpilib/math/estimator/KalmanFilterTest.java @@ -19,9 +19,9 @@ import org.wpilib.math.numbers.N1; import org.wpilib.math.numbers.N2; import org.wpilib.math.numbers.N3; import org.wpilib.math.numbers.N6; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.LinearSystem; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; +import org.wpilib.math.system.Models; import org.wpilib.math.trajectory.TrajectoryConfig; import org.wpilib.math.trajectory.TrajectoryGenerator; import org.wpilib.math.util.Nat; @@ -41,7 +41,7 @@ class KalmanFilterTest { var m = 5.0; var r = 0.0181864; var G = 1.0; - elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G); + elevatorPlant = Models.elevatorFromPhysicalConstants(motors, m, r, G); } // A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]ᵀ, diff --git a/wpimath/src/test/java/org/wpilib/math/estimator/MerweUKFTest.java b/wpimath/src/test/java/org/wpilib/math/estimator/MerweUKFTest.java index 709b1ee4d1..98d1f5bf4f 100644 --- a/wpimath/src/test/java/org/wpilib/math/estimator/MerweUKFTest.java +++ b/wpimath/src/test/java/org/wpilib/math/estimator/MerweUKFTest.java @@ -23,11 +23,11 @@ import org.wpilib.math.numbers.N3; import org.wpilib.math.numbers.N4; import org.wpilib.math.numbers.N5; import org.wpilib.math.random.Normal; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.Discretization; +import org.wpilib.math.system.Models; import org.wpilib.math.system.NumericalIntegration; import org.wpilib.math.system.NumericalJacobian; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; import org.wpilib.math.trajectory.TrajectoryConfig; import org.wpilib.math.trajectory.TrajectoryGenerator; import org.wpilib.math.util.Nat; @@ -222,7 +222,7 @@ class MerweUKFTest { @Test void testLinearUKF() { var dt = 0.020; - var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006); + var plant = Models.flywheelFromSysId(0.02, 0.006); var observer = new MerweUKF<>( Nat.N1(), diff --git a/wpimath/src/test/java/org/wpilib/math/estimator/S3UKFTest.java b/wpimath/src/test/java/org/wpilib/math/estimator/S3UKFTest.java index 8efca89c87..10e4d612ff 100644 --- a/wpimath/src/test/java/org/wpilib/math/estimator/S3UKFTest.java +++ b/wpimath/src/test/java/org/wpilib/math/estimator/S3UKFTest.java @@ -23,11 +23,11 @@ import org.wpilib.math.numbers.N3; import org.wpilib.math.numbers.N4; import org.wpilib.math.numbers.N5; import org.wpilib.math.random.Normal; +import org.wpilib.math.system.DCMotor; import org.wpilib.math.system.Discretization; +import org.wpilib.math.system.Models; import org.wpilib.math.system.NumericalIntegration; import org.wpilib.math.system.NumericalJacobian; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; import org.wpilib.math.trajectory.TrajectoryConfig; import org.wpilib.math.trajectory.TrajectoryGenerator; import org.wpilib.math.util.Nat; @@ -222,7 +222,7 @@ class S3UKFTest { @Test void testLinearUKF() { var dt = 0.020; - var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006); + var plant = Models.flywheelFromSysId(0.02, 0.006); var observer = new S3UKF<>( Nat.N1(), diff --git a/wpimath/src/test/java/org/wpilib/math/system/LinearSystemIDTest.java b/wpimath/src/test/java/org/wpilib/math/system/ModelsTest.java similarity index 63% rename from wpimath/src/test/java/org/wpilib/math/system/LinearSystemIDTest.java rename to wpimath/src/test/java/org/wpilib/math/system/ModelsTest.java index e3318dfb90..eebfd66f74 100644 --- a/wpimath/src/test/java/org/wpilib/math/system/LinearSystemIDTest.java +++ b/wpimath/src/test/java/org/wpilib/math/system/ModelsTest.java @@ -9,17 +9,36 @@ import static org.junit.jupiter.api.Assertions.assertTrue; import org.junit.jupiter.api.Test; import org.wpilib.math.linalg.MatBuilder; -import org.wpilib.math.linalg.Matrix; import org.wpilib.math.linalg.VecBuilder; -import org.wpilib.math.system.plant.DCMotor; -import org.wpilib.math.system.plant.LinearSystemId; import org.wpilib.math.util.Nat; -class LinearSystemIDTest { +class ModelsTest { @Test - void testDrivetrainVelocitySystem() { + void testFlywheelFromPhysicalConstants() { + var model = Models.flywheelFromPhysicalConstants(DCMotor.getNEO(2), 0.00032, 1.0); + assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001)); + + assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001)); + + assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001)); + + assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001)); + } + + @Test + void testFlywheelFromSysId() { + var kv = 1.0; + var ka = 0.5; + var model = Models.flywheelFromSysId(kv, ka); + + assertEquals(model.getA(), VecBuilder.fill(-kv / ka)); + assertEquals(model.getB(), VecBuilder.fill(1 / ka)); + } + + @Test + void testDifferentialDriveFromPhysicalConstants() { var model = - LinearSystemId.createDrivetrainVelocitySystem(DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6); + Models.differentialDriveFromPhysicalConstants(DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6); assertTrue( model .getA() @@ -41,8 +60,8 @@ class LinearSystemIDTest { } @Test - void testElevatorSystem() { - var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12); + void testElevatorFromPhysicalConstants() { + var model = Models.elevatorFromPhysicalConstants(DCMotor.getNEO(2), 5, 0.05, 12); assertTrue( model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -99.05473), 0.001)); @@ -54,52 +73,24 @@ class LinearSystemIDTest { } @Test - void testFlywheelSystem() { - var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0); - assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001)); - - assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001)); - - assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001)); - - assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001)); - } - - @Test - void testDCMotorSystem() { - var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0); - assertTrue( - model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -26.87032), 0.001)); - - assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001)); - - assertTrue(model.getC().isEqual(Matrix.eye(Nat.N2()), 0.001)); - - assertTrue(model.getD().isEqual(new Matrix<>(Nat.N2(), Nat.N1()), 0.001)); - } - - @Test - void testIdentifyPositionSystem() { - // By controls engineering in frc, - // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u + void testElevatorFromSysId() { var kv = 1.0; var ka = 0.5; - var model = LinearSystemId.identifyPositionSystem(kv, ka); + var model = Models.elevatorFromSysId(kv, ka); assertEquals(model.getA(), MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kv / ka)); assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka)); } @Test - void testIdentifyVelocitySystem() { - // By controls engineering in frc, - // V = kv * velocity + ka * acceleration - // x-dot = -kv/ka * v + 1/ka \cdot V + void testSingleJointedArmFromSysId() { var kv = 1.0; var ka = 0.5; - var model = LinearSystemId.identifyVelocitySystem(kv, ka); - assertEquals(model.getA(), VecBuilder.fill(-kv / ka)); - assertEquals(model.getB(), VecBuilder.fill(1 / ka)); + var model = Models.singleJointedArmFromSysId(kv, ka); + + assertTrue( + model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 0.0, -kv / ka), 0.001)); + assertTrue(model.getB().isEqual(MatBuilder.fill(Nat.N2(), Nat.N1(), 0.0, 1.0 / ka), 0.001)); } } diff --git a/wpimath/src/test/java/org/wpilib/math/system/plant/proto/DCMotorProtoTest.java b/wpimath/src/test/java/org/wpilib/math/system/proto/DCMotorProtoTest.java similarity index 87% rename from wpimath/src/test/java/org/wpilib/math/system/plant/proto/DCMotorProtoTest.java rename to wpimath/src/test/java/org/wpilib/math/system/proto/DCMotorProtoTest.java index d37de07977..36a360e341 100644 --- a/wpimath/src/test/java/org/wpilib/math/system/plant/proto/DCMotorProtoTest.java +++ b/wpimath/src/test/java/org/wpilib/math/system/proto/DCMotorProtoTest.java @@ -2,13 +2,13 @@ // 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. -package org.wpilib.math.system.plant.proto; +package org.wpilib.math.system.proto; import static org.junit.jupiter.api.Assertions.assertEquals; import org.junit.jupiter.api.Test; -import org.wpilib.math.proto.Plant.ProtobufDCMotor; -import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.proto.System.ProtobufDCMotor; +import org.wpilib.math.system.DCMotor; class DCMotorProtoTest { private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3); diff --git a/wpimath/src/test/java/org/wpilib/math/system/plant/struct/DCMotorStructTest.java b/wpimath/src/test/java/org/wpilib/math/system/struct/DCMotorStructTest.java similarity index 92% rename from wpimath/src/test/java/org/wpilib/math/system/plant/struct/DCMotorStructTest.java rename to wpimath/src/test/java/org/wpilib/math/system/struct/DCMotorStructTest.java index 18e951f455..bee53dde28 100644 --- a/wpimath/src/test/java/org/wpilib/math/system/plant/struct/DCMotorStructTest.java +++ b/wpimath/src/test/java/org/wpilib/math/system/struct/DCMotorStructTest.java @@ -2,14 +2,14 @@ // 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. -package org.wpilib.math.system.plant.struct; +package org.wpilib.math.system.struct; import static org.junit.jupiter.api.Assertions.assertEquals; import java.nio.ByteBuffer; import java.nio.ByteOrder; import org.junit.jupiter.api.Test; -import org.wpilib.math.system.plant.DCMotor; +import org.wpilib.math.system.DCMotor; class DCMotorStructTest { private static final DCMotor DATA = new DCMotor(1.91, 19.1, 1.74, 1.74, 22.9, 3); diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp index 7c06c865a2..841597e6d3 100644 --- a/wpimath/src/test/native/cpp/StateSpaceTest.cpp +++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp @@ -11,10 +11,10 @@ #include "wpi/math/controller/LinearQuadraticRegulator.hpp" #include "wpi/math/estimator/KalmanFilter.hpp" #include "wpi/math/linalg/EigenCore.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/LinearSystem.hpp" #include "wpi/math/system/LinearSystemLoop.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/units/time.hpp" namespace wpi::math { @@ -36,7 +36,8 @@ class StateSpaceTest : public testing::Test { // Gear ratio constexpr double G = 40.0 / 40.0; - return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0); + return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G) + .Slice(0); }(); LinearQuadraticRegulator<2, 1> controller{plant, {0.02, 0.4}, {12.0}, kDt}; KalmanFilter<2, 1, 1> observer{plant, {0.05, 1.0}, {0.0001}, kDt}; diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp index f30ababa58..97dd62cad4 100644 --- a/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp +++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveAccelerationLimiterTest.cpp @@ -6,7 +6,7 @@ #include -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/units/math.hpp" namespace wpi::math { @@ -19,8 +19,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, LowLimits) { using Kv_t = decltype(1_V / 1_mps); using Ka_t = decltype(1_V / 1_mps_sq); - auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, - Kv_t{1.0}, Ka_t{1.0}); + auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, maxA, maxAlpha}; @@ -111,8 +111,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, HighLimits) { using Kv_t = decltype(1_V / 1_mps); using Ka_t = decltype(1_V / 1_mps_sq); - auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, - Kv_t{1.0}, Ka_t{1.0}); + auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); // Limits are so high, they don't get hit, so states of constrained and // unconstrained systems should match @@ -181,8 +181,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) { using Kv_t = decltype(1_V / 1_mps); using Ka_t = decltype(1_V / 1_mps_sq); - auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, - Kv_t{1.0}, Ka_t{1.0}); + auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); DifferentialDriveAccelerationLimiter accelLimiter{plant, trackwidth, minA, maxA, maxAlpha}; @@ -240,8 +240,8 @@ TEST(DifferentialDriveAccelerationLimiterTest, SeparateMinMaxLowLimits) { TEST(DifferentialDriveAccelerationLimiterTest, MinAccelGreaterThanMaxAccel) { using Kv_t = decltype(1_V / 1_mps); using Ka_t = decltype(1_V / 1_mps_sq); - auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, - Kv_t{1.0}, Ka_t{1.0}); + auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); EXPECT_NO_THROW({ DifferentialDriveAccelerationLimiter accelLimiter(plant, 1_m, 1_mps_sq, 1_rad_per_s_sq); diff --git a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp index ef481e1f97..4cf4ba1330 100644 --- a/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp @@ -4,13 +4,10 @@ #include "wpi/math/controller/DifferentialDriveFeedforward.hpp" -#include - #include #include -#include "wpi/math/controller/LinearPlantInversionFeedforward.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/units/acceleration.hpp" #include "wpi/units/length.hpp" #include "wpi/units/time.hpp" @@ -26,7 +23,7 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithTrackwidth) { wpi::math::DifferentialDriveFeedforward differentialDriveFeedforward{ kVLinear, kALinear, kVAngular, kAAngular, trackwidth}; wpi::math::LinearSystem<2, 2, 2> plant = - wpi::math::LinearSystemId::IdentifyDrivetrainSystem( + wpi::math::Models::DifferentialDriveFromSysId( kVLinear, kALinear, kVAngular, kAAngular, trackwidth); for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps; currentLeftVelocity += 2_mps) { @@ -60,8 +57,8 @@ TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) { wpi::math::DifferentialDriveFeedforward differentialDriveFeedforward{ kVLinear, kALinear, kVAngular, kAAngular}; wpi::math::LinearSystem<2, 2, 2> plant = - wpi::math::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear, - kVAngular, kAAngular); + wpi::math::Models::DifferentialDriveFromSysId(kVLinear, kALinear, + kVAngular, kAAngular); for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps; currentLeftVelocity += 2_mps) { for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps; diff --git a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp index 09e187c707..cb23c0a09e 100644 --- a/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ElevatorFeedforwardTest.cpp @@ -4,15 +4,13 @@ #include "wpi/math/controller/ElevatorFeedforward.hpp" -#include - #include #include "wpi/math/controller/LinearPlantInversionFeedforward.hpp" #include "wpi/math/linalg/EigenCore.hpp" -#include "wpi/units/acceleration.hpp" #include "wpi/units/length.hpp" #include "wpi/units/time.hpp" +#include "wpi/units/velocity.hpp" static constexpr auto Ks = 0.5_V; static constexpr auto Kv = 1.5_V * 1_s / 1_m; diff --git a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp index d87dbbbbe9..e061b75736 100644 --- a/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/ImplicitModelFollowerTest.cpp @@ -6,7 +6,7 @@ #include -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" namespace wpi::math { @@ -15,8 +15,8 @@ TEST(ImplicitModelFollowerTest, SameModel) { using Kv_t = decltype(1_V / 1_mps); using Ka_t = decltype(1_V / 1_mps_sq); - auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, - Kv_t{1.0}, Ka_t{1.0}); + auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); ImplicitModelFollower<2, 2> imf{plant, plant}; @@ -60,12 +60,12 @@ TEST(ImplicitModelFollowerTest, SlowerRefModel) { using Kv_t = decltype(1_V / 1_mps); using Ka_t = decltype(1_V / 1_mps_sq); - auto plant = LinearSystemId::IdentifyDrivetrainSystem(Kv_t{1.0}, Ka_t{1.0}, - Kv_t{1.0}, Ka_t{1.0}); + auto plant = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{1.0}, + Kv_t{1.0}, Ka_t{1.0}); // Linear acceleration is slower, but angular acceleration is the same - auto plantRef = LinearSystemId::IdentifyDrivetrainSystem( - Kv_t{1.0}, Ka_t{2.0}, Kv_t{1.0}, Ka_t{1.0}); + auto plantRef = Models::DifferentialDriveFromSysId(Kv_t{1.0}, Ka_t{2.0}, + Kv_t{1.0}, Ka_t{1.0}); ImplicitModelFollower<2, 2> imf{plant, plantRef}; diff --git a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp index a9161790c1..ebd5e53d49 100644 --- a/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LTVDifferentialDriveControllerTest.cpp @@ -8,8 +8,8 @@ #include +#include "wpi/math/system/Models.hpp" #include "wpi/math/system/NumericalIntegration.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" #include "wpi/math/trajectory/TrajectoryGenerator.hpp" #include "wpi/math/util/MathUtil.hpp" #include "wpi/units/math.hpp" @@ -46,7 +46,7 @@ static constexpr auto kLinearV = 3.02_V / 1_mps; static constexpr auto kLinearA = 0.642_V / 1_mps_sq; static constexpr auto kAngularV = 1.382_V / 1_mps; static constexpr auto kAngularA = 0.08495_V / 1_mps_sq; -static auto plant = wpi::math::LinearSystemId::IdentifyDrivetrainSystem( +static auto plant = wpi::math::Models::DifferentialDriveFromSysId( kLinearV, kLinearA, kAngularV, kAngularA); static constexpr auto kTrackwidth = 0.9_m; diff --git a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp index da2a0a3494..ed1a95214b 100644 --- a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp @@ -4,8 +4,6 @@ #include "wpi/math/controller/LinearPlantInversionFeedforward.hpp" -#include - #include #include "wpi/math/linalg/EigenCore.hpp" diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp index 5f5f3c6266..b8f46ee605 100644 --- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp @@ -4,14 +4,12 @@ #include "wpi/math/controller/LinearQuadraticRegulator.hpp" -#include - #include #include "wpi/math/linalg/EigenCore.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/LinearSystem.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/units/time.hpp" namespace wpi::math { @@ -29,7 +27,8 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) { // Gear ratio constexpr double G = 40.0 / 40.0; - return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0); + return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G) + .Slice(0); }(); Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5_ms}.K(); @@ -51,7 +50,7 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) { // Gear ratio constexpr double G = 100.0; - return wpi::math::LinearSystemId::SingleJointedArmSystem( + return wpi::math::Models::SingleJointedArmFromPhysicalConstants( motors, 1.0 / 3.0 * m * r * r, G) .Slice(0); }(); @@ -77,7 +76,8 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) { // Gear ratio constexpr double G = 14.67; - return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0); + return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G) + .Slice(0); }(); Matrixd<1, 2> K = LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K(); @@ -179,7 +179,8 @@ TEST(LinearQuadraticRegulatorTest, LatencyCompensate) { // Gear ratio constexpr double G = 14.67; - return wpi::math::LinearSystemId::ElevatorSystem(motors, m, r, G).Slice(0); + return wpi::math::Models::ElevatorFromPhysicalConstants(motors, m, r, G) + .Slice(0); }(); LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 20_ms}; diff --git a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp index 1e6da06d99..23930e21fb 100644 --- a/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp +++ b/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp @@ -4,8 +4,6 @@ #include "wpi/math/controller/SimpleMotorFeedforward.hpp" -#include - #include #include "wpi/math/controller/LinearPlantInversionFeedforward.hpp" diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp index 24d366e7af..66f34cea53 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp @@ -16,7 +16,6 @@ #include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/math/kinematics/DifferentialDriveKinematics.hpp" #include "wpi/math/trajectory/TrajectoryGenerator.hpp" -#include "wpi/math/util/StateSpaceUtil.hpp" #include "wpi/units/angle.hpp" #include "wpi/units/length.hpp" #include "wpi/units/time.hpp" diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index 0bfb579c0e..00fce96313 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -17,7 +17,6 @@ #include "wpi/math/geometry/Rotation2d.hpp" #include "wpi/math/kinematics/DifferentialDriveKinematics.hpp" #include "wpi/math/trajectory/TrajectoryGenerator.hpp" -#include "wpi/math/util/StateSpaceUtil.hpp" #include "wpi/units/angle.hpp" #include "wpi/units/length.hpp" #include "wpi/units/time.hpp" diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp index 623a8b2a92..bc9ad12f63 100644 --- a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp @@ -12,8 +12,8 @@ #include "wpi/math/linalg/EigenCore.hpp" #include "wpi/math/random/Normal.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/NumericalJacobian.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" #include "wpi/math/trajectory/TrajectoryGenerator.hpp" #include "wpi/units/moment_of_inertia.hpp" diff --git a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp index a2fef6ebe8..4ad7f89d07 100644 --- a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp @@ -10,14 +10,14 @@ #include #include -#include "wpi/math/system/plant/DCMotor.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/DCMotor.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/units/moment_of_inertia.hpp" #include "wpi/units/time.hpp" TEST(KalmanFilterTest, Flywheel) { auto motor = wpi::math::DCMotor::NEO(); auto flywheel = - wpi::math::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0); + wpi::math::Models::FlywheelFromPhysicalConstants(motor, 1_kg_sq_m, 1.0); wpi::math::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms}; } diff --git a/wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp index 99718c1a0a..25b73fa376 100644 --- a/wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MerweUKFTest.cpp @@ -15,11 +15,11 @@ #include "wpi/math/estimator/AngleStatistics.hpp" #include "wpi/math/linalg/EigenCore.hpp" #include "wpi/math/random/Normal.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/Discretization.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/math/system/NumericalIntegration.hpp" #include "wpi/math/system/NumericalJacobian.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" #include "wpi/math/trajectory/TrajectoryGenerator.hpp" #include "wpi/units/moment_of_inertia.hpp" @@ -183,9 +183,8 @@ TEST(MerweUKFTest, DriveConvergence) { TEST(MerweUKFTest, LinearUKF) { constexpr wpi::units::second_t dt = 20_ms; - auto plant = - wpi::math::LinearSystemId::IdentifyVelocitySystem( - 0.02_V / 1_mps, 0.006_V / 1_mps_sq); + auto plant = wpi::math::Models::FlywheelFromSysId(0.02_V / 1_rad_per_s, + 0.006_V / 1_rad_per_s_sq); wpi::math::MerweUKF<1, 1, 1> observer{ [&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) { return plant.A() * x + plant.B() * u; diff --git a/wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp b/wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp index 2ae02dcede..4ca6eb20c2 100644 --- a/wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/S3UKFTest.cpp @@ -15,11 +15,11 @@ #include "wpi/math/estimator/AngleStatistics.hpp" #include "wpi/math/linalg/EigenCore.hpp" #include "wpi/math/random/Normal.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/Discretization.hpp" +#include "wpi/math/system/Models.hpp" #include "wpi/math/system/NumericalIntegration.hpp" #include "wpi/math/system/NumericalJacobian.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" -#include "wpi/math/system/plant/LinearSystemId.hpp" #include "wpi/math/trajectory/TrajectoryGenerator.hpp" #include "wpi/units/moment_of_inertia.hpp" @@ -183,9 +183,8 @@ TEST(S3UKFTest, DriveConvergence) { TEST(S3UKFTest, LinearUKF) { constexpr wpi::units::second_t dt = 20_ms; - auto plant = - wpi::math::LinearSystemId::IdentifyVelocitySystem( - 0.02_V / 1_mps, 0.006_V / 1_mps_sq); + auto plant = wpi::math::Models::FlywheelFromSysId(0.02_V / 1_rad_per_s, + 0.006_V / 1_rad_per_s_sq); wpi::math::S3UKF<1, 1, 1> observer{ [&](const wpi::math::Vectord<1>& x, const wpi::math::Vectord<1>& u) { return plant.A() * x + plant.B() * u; diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp index 94c9678670..a29f8abffc 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp @@ -16,7 +16,6 @@ #include "wpi/math/kinematics/SwerveDriveKinematics.hpp" #include "wpi/math/trajectory/TrajectoryGenerator.hpp" #include "wpi/util/print.hpp" -#include "wpi/util/timestamp.h" void testFollowTrajectory( const wpi::math::SwerveDriveKinematics<4>& kinematics, diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index 1af5fe0936..527b97ed10 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -17,7 +17,6 @@ #include "wpi/math/kinematics/SwerveDriveKinematics.hpp" #include "wpi/math/trajectory/TrajectoryGenerator.hpp" #include "wpi/util/print.hpp" -#include "wpi/util/timestamp.h" void testFollowTrajectory( const wpi::math::SwerveDriveKinematics<4>& kinematics, diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIdTest.cpp b/wpimath/src/test/native/cpp/system/ModelsTest.cpp similarity index 53% rename from wpimath/src/test/native/cpp/system/LinearSystemIdTest.cpp rename to wpimath/src/test/native/cpp/system/ModelsTest.cpp index 402c8d7005..5bdd1a05ec 100644 --- a/wpimath/src/test/native/cpp/system/LinearSystemIdTest.cpp +++ b/wpimath/src/test/native/cpp/system/ModelsTest.cpp @@ -2,22 +2,54 @@ // 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 "wpi/math/system/plant/LinearSystemId.hpp" +#include "wpi/math/system/Models.hpp" #include +#include "wpi/math/system/DCMotor.hpp" #include "wpi/math/system/LinearSystem.hpp" -#include "wpi/math/system/plant/DCMotor.hpp" #include "wpi/units/length.hpp" #include "wpi/units/mass.hpp" -TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) { +TEST(ModelsTest, FlywheelFromPhysicalConstants) { #if __GNUC__ <= 11 - auto model = wpi::math::LinearSystemId::DrivetrainVelocitySystem( + auto model = wpi::math::Models::FlywheelFromPhysicalConstants( + wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0); +#else + constexpr auto model = wpi::math::Models::FlywheelFromPhysicalConstants( + wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0); +#endif + + ASSERT_TRUE(model.A().isApprox(wpi::math::Matrixd<1, 1>{-26.87032}, 0.001)); + ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<1, 1>{1354.166667}, 0.001)); + ASSERT_TRUE(model.C().isApprox(wpi::math::Matrixd<1, 1>{1.0}, 0.001)); + ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<1, 1>{0.0}, 0.001)); +} + +TEST(ModelsTest, FlywheelFromSysId) { + constexpr double kv = 1.0; + constexpr double ka = 0.5; + +#if __GNUC__ <= 11 + auto model = wpi::math::Models::FlywheelFromSysId(kv * 1_V / 1_rad_per_s, + ka * 1_V / 1_rad_per_s_sq); +#else + constexpr auto model = wpi::math::Models::FlywheelFromSysId( + kv * 1_V / 1_rad_per_s, ka * 1_V / 1_rad_per_s_sq); +#endif + + ASSERT_TRUE(model.A().isApprox(wpi::math::Matrixd<1, 1>{-kv / ka}, 0.001)); + ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<1, 1>{1.0 / ka}, 0.001)); +} + +TEST(ModelsTest, DifferentialDriveFromPhysicalConstants) { +#if __GNUC__ <= 11 + auto model = wpi::math::Models::DifferentialDriveFromPhysicalConstants( wpi::math::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0); #else - constexpr auto model = wpi::math::LinearSystemId::DrivetrainVelocitySystem( - wpi::math::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0); + constexpr auto model = + wpi::math::Models::DifferentialDriveFromPhysicalConstants( + wpi::math::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0); #endif ASSERT_TRUE(model.A().isApprox( @@ -31,8 +63,8 @@ TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) { wpi::math::Matrixd<2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001)); } -TEST(LinearSystemIDTest, ElevatorSystem) { - auto model = wpi::math::LinearSystemId::ElevatorSystem( +TEST(ModelsTest, ElevatorFromPhysicalConstants) { + auto model = wpi::math::Models::ElevatorFromPhysicalConstants( wpi::math::DCMotor::NEO(2), 5_kg, 0.05_m, 12) .Slice(0); ASSERT_TRUE(model.A().isApprox( @@ -42,53 +74,16 @@ TEST(LinearSystemIDTest, ElevatorSystem) { ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<1, 1>{0.0}, 0.001)); } -TEST(LinearSystemIDTest, FlywheelSystem) { -#if __GNUC__ <= 11 - auto model = wpi::math::LinearSystemId::FlywheelSystem( - wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0); -#else - constexpr auto model = wpi::math::LinearSystemId::FlywheelSystem( - wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0); -#endif - - ASSERT_TRUE(model.A().isApprox(wpi::math::Matrixd<1, 1>{-26.87032}, 0.001)); - ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<1, 1>{1354.166667}, 0.001)); - ASSERT_TRUE(model.C().isApprox(wpi::math::Matrixd<1, 1>{1.0}, 0.001)); - ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<1, 1>{0.0}, 0.001)); -} - -TEST(LinearSystemIDTest, DCMotorSystem) { -#if __GNUC__ <= 11 - auto model = wpi::math::LinearSystemId::DCMotorSystem( - wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0); -#else - constexpr auto model = wpi::math::LinearSystemId::DCMotorSystem( - wpi::math::DCMotor::NEO(2), 0.00032_kg_sq_m, 1.0); -#endif - - ASSERT_TRUE(model.A().isApprox( - wpi::math::Matrixd<2, 2>{{0, 1}, {0, -26.87032}}, 0.001)); - ASSERT_TRUE( - model.B().isApprox(wpi::math::Matrixd<2, 1>{0, 1354.166667}, 0.001)); - ASSERT_TRUE(model.C().isApprox( - wpi::math::Matrixd<2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001)); - ASSERT_TRUE(model.D().isApprox(wpi::math::Matrixd<2, 1>{0.0, 0.0}, 0.001)); -} - -TEST(LinearSystemIDTest, IdentifyPositionSystem) { - // By controls engineering in frc, - // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u +TEST(ModelsTest, ElevatorFromSysId) { constexpr double kv = 1.0; constexpr double ka = 0.5; #if __GNUC__ <= 11 - auto model = - wpi::math::LinearSystemId::IdentifyPositionSystem( - kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); + auto model = wpi::math::Models::ElevatorFromSysId(kv * 1_V / 1_mps, + ka * 1_V / 1_mps_sq); #else - constexpr auto model = - wpi::math::LinearSystemId::IdentifyPositionSystem( - kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); + constexpr auto model = wpi::math::Models::ElevatorFromSysId( + kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); #endif ASSERT_TRUE(model.A().isApprox( @@ -97,23 +92,20 @@ TEST(LinearSystemIDTest, IdentifyPositionSystem) { model.B().isApprox(wpi::math::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001)); } -TEST(LinearSystemIDTest, IdentifyVelocitySystem) { - // By controls engineering in frc, - // V = kv * velocity + ka * acceleration - // x-dot = -kv/ka * v + 1/ka \cdot V +TEST(ModelsTest, SingleJointedArmFromSysId) { constexpr double kv = 1.0; constexpr double ka = 0.5; #if __GNUC__ <= 11 - auto model = - wpi::math::LinearSystemId::IdentifyVelocitySystem( - kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); + auto model = wpi::math::Models::SingleJointedArmFromSysId( + kv * 1_V / 1_rad_per_s, ka * 1_V / 1_rad_per_s_sq); #else - constexpr auto model = - wpi::math::LinearSystemId::IdentifyVelocitySystem( - kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq); + constexpr auto model = wpi::math::Models::SingleJointedArmFromSysId( + kv * 1_V / 1_rad_per_s, ka * 1_V / 1_rad_per_s_sq); #endif - ASSERT_TRUE(model.A().isApprox(wpi::math::Matrixd<1, 1>{-kv / ka}, 0.001)); - ASSERT_TRUE(model.B().isApprox(wpi::math::Matrixd<1, 1>{1.0 / ka}, 0.001)); + ASSERT_TRUE(model.A().isApprox( + wpi::math::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001)); + ASSERT_TRUE( + model.B().isApprox(wpi::math::Matrixd<2, 1>{0.0, 1.0 / ka}, 0.001)); } diff --git a/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp b/wpimath/src/test/native/cpp/system/proto/DCMotorProtoTest.cpp similarity index 96% rename from wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp rename to wpimath/src/test/native/cpp/system/proto/DCMotorProtoTest.cpp index ce65cced7a..112cedb62b 100644 --- a/wpimath/src/test/native/cpp/system/plant/proto/DCMotorProtoTest.cpp +++ b/wpimath/src/test/native/cpp/system/proto/DCMotorProtoTest.cpp @@ -4,7 +4,7 @@ #include -#include "wpi/math/system/plant/DCMotor.hpp" +#include "wpi/math/system/DCMotor.hpp" #include "wpi/util/SmallVector.hpp" using namespace wpi::math; diff --git a/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp b/wpimath/src/test/native/cpp/system/struct/DCMotorStructTest.cpp similarity index 96% rename from wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp rename to wpimath/src/test/native/cpp/system/struct/DCMotorStructTest.cpp index ac66b326cf..63a77c71bc 100644 --- a/wpimath/src/test/native/cpp/system/plant/struct/DCMotorStructTest.cpp +++ b/wpimath/src/test/native/cpp/system/struct/DCMotorStructTest.cpp @@ -4,7 +4,7 @@ #include -#include "wpi/math/system/plant/DCMotor.hpp" +#include "wpi/math/system/DCMotor.hpp" using namespace wpi::math;