Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add RKF45 integration (#3047)

Browse Source 
This is more stable than Runge-Kutta for systems with large elements in their A or B matrices.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Matt
2021-01-06 21:40:25 -08:00
committed by GitHub
parent 278e0f126e
commit 85a0bd43c2
25 changed files with 560 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
View File

@@ -13,6 +13,7 @@
#include "frc/controller/PIDController.h"
#include "frc/simulation/ElevatorSim.h"
#include "frc/simulation/EncoderSim.h"
#include "frc/system/NumericalIntegration.h"
#include "frc/system/plant/DCMotor.h"
#include "frc/system/plant/LinearSystemId.h"
#include "gtest/gtest.h"
@@ -64,3 +65,28 @@ TEST(ElevatorSim, MinMax) {
EXPECT_TRUE(height < 1.05_m);
}
}
TEST(ElevatorSim, Stability) {
static constexpr double kElevatorGearing = 100.0;
static constexpr units::meter_t kElevatorDrumRadius = 0.5_in;
static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem(
m_elevatorGearbox, kCarriageMass, kElevatorDrumRadius, kElevatorGearing);
Eigen::Matrix<double, 2, 1> x0 = frc::MakeMatrix<2, 1>(0.0, 0.0);
Eigen::Matrix<double, 1, 1> u0 = frc::MakeMatrix<1, 1>(12.0);
Eigen::Matrix<double, 2, 1> x1 = frc::MakeMatrix<2, 1>(0.0, 0.0);
for (size_t i = 0; i < 50; i++) {
x1 = frc::RKF45(
[&](Eigen::Matrix<double, 2, 1> x,
Eigen::Matrix<double, 1, 1> u) -> Eigen::Matrix<double, 2, 1> {
return system.A() * x + system.B() * u;
},
x1, u0, 0.020_s);
}
EXPECT_NEAR(x1(0), system.CalculateX(x0, u0, 1_s)(0), 0.1);
}