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

18
wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
View File

@@ -80,29 +80,27 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const {
}
void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
SetInput(Eigen::Vector<double, 1>{voltage.value()});
SetInput(Vectord<1>{voltage.value()});
}
Eigen::Vector<double, 2> ElevatorSim::UpdateX(
const Eigen::Vector<double, 2>& currentXhat,
const Eigen::Vector<double, 1>& u, units::second_t dt) {
Vectord<2> ElevatorSim::UpdateX(const Vectord<2>& currentXhat,
const Vectord<1>& u, units::second_t dt) {
auto updatedXhat = RKDP(
[&](const Eigen::Vector<double, 2>& x,
const Eigen::Vector<double, 1>& u_) -> Eigen::Vector<double, 2> {
Eigen::Vector<double, 2> xdot = m_plant.A() * x + m_plant.B() * u;
[&](const Vectord<2>& x, const Vectord<1>& u_) -> Vectord<2> {
Vectord<2> xdot = m_plant.A() * x + m_plant.B() * u;
if (m_simulateGravity) {
xdot += Eigen::Vector<double, 2>{0.0, -9.8};
xdot += Vectord<2>{0.0, -9.8};
}
return xdot;
},
currentXhat, u, dt);
// Check for collision after updating x-hat.
if (WouldHitLowerLimit(units::meter_t(updatedXhat(0)))) {
return Eigen::Vector<double, 2>{m_minHeight.value(), 0.0};
return Vectord<2>{m_minHeight.value(), 0.0};
}
if (WouldHitUpperLimit(units::meter_t(updatedXhat(0)))) {
return Eigen::Vector<double, 2>{m_maxHeight.value(), 0.0};
return Vectord<2>{m_maxHeight.value(), 0.0};
}
return updatedXhat;
}