Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31:44 +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) {

14
wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
View File

@@ -122,6 +122,20 @@ class DifferentialDrivetrainSim {
*/
units::ampere_t GetCurrentDraw() const;
/**
* Sets the system state.
*
* @param state The state.
*/
void SetState(const Eigen::Matrix<double, 7, 1>& state);
/**
* Sets the system pose.
*
* @param pose The pose.
*/
void SetPose(const frc::Pose2d& pose);
Eigen::Matrix<double, 7, 1> Dynamics(const Eigen::Matrix<double, 7, 1>& x,
const Eigen::Matrix<double, 2, 1>& u);

20
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
View File

@@ -174,6 +174,26 @@ public class DifferentialDrivetrainSim {
this.m_currentGearing = newGearRatio;
}
/**
* Sets the system state.
*
* @param state The state.
*/
public void setState(Matrix<N7, N1> state) {
m_x = state;
}
/**
* Sets the system pose.
*
* @param pose The pose.
*/
public void setPose(Pose2d pose) {
m_x.set(State.kX.value, 0, pose.getX());
m_x.set(State.kY.value, 0, pose.getY());
m_x.set(State.kHeading.value, 0, pose.getRotation().getRadians());
}
@SuppressWarnings({"DuplicatedCode", "LocalVariableName"})
protected Matrix<N7, N1> getDynamics(Matrix<N7, N1> x, Matrix<N2, N1> u) {