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

[wpimath] Fully discretized ElevatorFF and ArmFF (#7024)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-10-11 01:10:45 -04:00
committed by GitHub
parent 5d9a553104
commit 4adfa8bf64
31 changed files with 1004 additions and 425 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -19,6 +19,19 @@ units::volt_t ArmFeedforward::Calculate(units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity,
units::second_t dt) const {
return Calculate(currentAngle, currentVelocity, nextVelocity);
}
units::volt_t ArmFeedforward::Calculate(
units::unit_t<Angle> currentAngle,
units::unit_t<Velocity> currentVelocity) const {
return kS * wpi::sgn(currentVelocity) + kG * units::math::cos(currentAngle) +
kV * currentVelocity;
}
units::volt_t ArmFeedforward::Calculate(
units::unit_t<Angle> currentAngle, units::unit_t<Velocity> currentVelocity,
units::unit_t<Velocity> nextVelocity) const {
using VarMat = sleipnir::VariableMatrix;
// Arm dynamics
@@ -36,12 +49,12 @@ units::volt_t ArmFeedforward::Calculate(units::unit_t<Angle> currentAngle,
sleipnir::Variable u_k;
// Initial guess
auto acceleration = (nextVelocity - currentVelocity) / dt;
auto acceleration = (nextVelocity - currentVelocity) / m_dt;
u_k.SetValue((kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
kA * acceleration + kG * units::math::cos(currentAngle))
.value());
auto r_k1 = RK4<decltype(f), VarMat, VarMat>(f, r_k, u_k, dt);
auto r_k1 = RK4<decltype(f), VarMat, VarMat>(f, r_k, u_k, m_dt);
// Minimize difference between desired and actual next velocity
auto cost =

5
wpimath/src/main/native/cpp/jni/ArmFeedforwardJNI.cpp
View File

@@ -26,10 +26,11 @@ Java_edu_wpi_first_math_jni_ArmFeedforwardJNI_calculate
{
return frc::ArmFeedforward{units::volt_t{ks}, units::volt_t{kg},
units::unit_t<frc::ArmFeedforward::kv_unit>{kv},
units::unit_t<frc::ArmFeedforward::ka_unit>{ka}}
units::unit_t<frc::ArmFeedforward::ka_unit>{ka},
units::second_t{dt}}
.Calculate(units::radian_t{currentAngle},
units::radians_per_second_t{currentVelocity},
units::radians_per_second_t{nextVelocity}, units::second_t{dt})
units::radians_per_second_t{nextVelocity})
.value();
}