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] Add typedefs for common types

Browse Source 
This makes complex code significantly easier to read.

frc::Vectord<Size> = Eigen::Vector<double, Size>
frc::Matrixd<Rows, Cols> = Eigen::Matrix<double, Rows, Cols>
This commit is contained in:
Peter Johnson
2022-04-29 22:29:20 -07:00
parent 97c493241f
commit e767605e94
76 changed files with 1136 additions and 1449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -24,21 +24,21 @@ DifferentialDriveAccelerationLimiter::Calculate(
units::meters_per_second_t leftVelocity,
units::meters_per_second_t rightVelocity, units::volt_t leftVoltage,
units::volt_t rightVoltage) {
Eigen::Vector<double, 2> u{leftVoltage.value(), rightVoltage.value()};
Vectord<2> u{leftVoltage.value(), rightVoltage.value()};
// Find unconstrained wheel accelerations
Eigen::Vector<double, 2> x{leftVelocity.value(), rightVelocity.value()};
Eigen::Vector<double, 2> dxdt = m_system.A() * x + m_system.B() * u;
Vectord<2> x{leftVelocity.value(), rightVelocity.value()};
Vectord<2> dxdt = m_system.A() * x + m_system.B() * u;
// Converts from wheel accelerations to linear and angular acceleration
// a = (dxdt(0) + dxdt(1)) / 2.0
// alpha = (dxdt(1) - dxdt(0)) / trackwidth
Eigen::Matrix<double, 2, 2> M{
{0.5, 0.5}, {-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
Matrixd<2, 2> M{{0.5, 0.5},
{-1.0 / m_trackwidth.value(), 1.0 / m_trackwidth.value()}};
// Convert to linear and angular accelerations, constrain them, then convert
// back
Eigen::Vector<double, 2> accels = M * dxdt;
Vectord<2> accels = M * dxdt;
if (accels(0) > m_maxLinearAccel.value()) {
accels(0) = m_maxLinearAccel.value();
} else if (accels(0) < -m_maxLinearAccel.value()) {