[wpimath] Reorganize LinearSystem factories (#8468)

This commit is contained in:
Tyler Veness
2026-01-12 19:09:35 -08:00
committed by GitHub
parent 89d0759ef2
commit 00fa8361dd
108 changed files with 1808 additions and 2138 deletions

View File

@@ -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};

View File

@@ -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};

View File

@@ -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(

View File

@@ -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<wpi::units::radian>(
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<wpi::units::radian> feedforward{