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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-04-25 23:45:43 -07:00
parent 2f0990e9d2 0695a4db89
commit 36811211be
79 changed files with 2093 additions and 415 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
View File

@@ -64,8 +64,14 @@ units::volt_t ArmFeedforward::Calculate(
sleipnir::Hessian hessianF{cost, xAD};
Eigen::SparseMatrix<double> H = hessianF.Value();
double error = std::numeric_limits<double>::infinity();
while (error > 1e-8) {
double error_k = std::numeric_limits<double>::infinity();
double error_k1 = std::abs(g.coeff(0));
// Loop until error stops decreasing or max iterations is reached
for (size_t iteration = 0;
iteration < 50 && error_k1 < (1.0 - 1e-10) * error_k; ++iteration) {
error_k = error_k1;
// Iterate via Newton's method.
//
// xₖ₊₁ = xₖ − H⁻¹g
@@ -97,7 +103,7 @@ units::volt_t ArmFeedforward::Calculate(
g = gradientF.Value();
H = hessianF.Value();
error = std::abs(g.coeff(0));
error_k1 = std::abs(g.coeff(0));
}
}

10
wpimath/src/main/native/cpp/filter/Debouncer.cpp
View File

@@ -10,15 +10,7 @@ using namespace frc;
Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
: m_debounceTime(debounceTime), m_debounceType(type) {
switch (type) {
case DebounceType::kBoth: // fall-through
case DebounceType::kRising:
m_baseline = false;
break;
case DebounceType::kFalling:
m_baseline = true;
break;
}
m_baseline = m_debounceType == DebounceType::kFalling;
ResetTimer();
}

11
wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp
View File

@@ -34,11 +34,12 @@ Translation2d Ellipse2d::Nearest(const Translation2d& point) const {
slp::pow(y - rotPoint.Y().value(), 2));
// (x − x_c)²/a² + (y − y_c)²/b² = 1
problem.SubjectTo(slp::pow(x - m_center.X().value(), 2) /
(m_xSemiAxis.value() * m_xSemiAxis.value()) +
slp::pow(y - m_center.Y().value(), 2) /
(m_ySemiAxis.value() * m_ySemiAxis.value()) ==
1);
// b²(x − x_c)² + a²(y − y_c)² = a²b²
double a2 = m_xSemiAxis.value() * m_xSemiAxis.value();
double b2 = m_ySemiAxis.value() * m_ySemiAxis.value();
problem.SubjectTo(b2 * slp::pow(x - m_center.X().value(), 2) +
a2 * slp::pow(y - m_center.Y().value(), 2) ==
a2 * b2);
problem.Solve();