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

[wpilib] Add set functions to differential drive simulation (#2746)

Browse Source 
All the other simulation objects already have functions like this
through LinearSystemSim.
This commit is contained in:
Tyler Veness
2020-09-27 13:26:47 -07:00
committed by GitHub
parent c55fb583b8
commit 62731bea20
3 changed files with 45 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
View File

@@ -89,6 +89,17 @@ units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
return loadIleft + loadIRight;
}
void DifferentialDrivetrainSim::SetState(
const Eigen::Matrix<double, 7, 1>& state) {
m_x = state;
}
void DifferentialDrivetrainSim::SetPose(const frc::Pose2d& pose) {
m_x(State::kX) = pose.X().to<double>();
m_x(State::kY) = pose.Y().to<double>();
m_x(State::kHeading) = pose.Rotation().Radians().to<double>();
}
Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::Dynamics(
const Eigen::Matrix<double, 7, 1>& x,
const Eigen::Matrix<double, 2, 1>& u) {