Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Clean up physics simulation class APIs (#2763)

Browse Source
This commit is contained in:
Prateek Machiraju
2020-10-16 00:00:45 -04:00
committed by GitHub
parent 8f3e5794b3
commit 061432147d
19 changed files with 746 additions and 483 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -16,15 +16,27 @@
using namespace frc;
using namespace frc::sim;
ElevatorSim::ElevatorSim(const DCMotor& gearbox, units::kilogram_t carriageMass,
double gearing, units::meter_t drumRadius,
units::meter_t minHeight, units::meter_t maxHeight,
bool addNoise,
const std::array<double, 1>& m_measurementStdDevs)
ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
const DCMotor& gearbox, double gearing,
units::meter_t drumRadius, units::meter_t minHeight,
units::meter_t maxHeight,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim(plant, measurementStdDevs),
m_gearbox(gearbox),
m_drumRadius(drumRadius),
m_minHeight(minHeight),
m_maxHeight(maxHeight),
m_gearing(gearing) {}
ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
units::kilogram_t carriageMass,
units::meter_t drumRadius, units::meter_t minHeight,
units::meter_t maxHeight,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
drumRadius, gearing),
addNoise, m_measurementStdDevs),
m_motor(gearbox),
measurementStdDevs),
m_gearbox(gearbox),
m_drumRadius(drumRadius),
m_minHeight(minHeight),
m_maxHeight(maxHeight),
@@ -38,7 +50,9 @@ bool ElevatorSim::HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const {
return x(0) > m_maxHeight.to<double>();
}
units::meter_t ElevatorSim::GetPosition() const { return units::meter_t{Y(0)}; }
units::meter_t ElevatorSim::GetPosition() const {
return units::meter_t{m_x(0)};
}
units::meters_per_second_t ElevatorSim::GetVelocity() const {
return units::meters_per_second_t{m_x(1)};
@@ -55,10 +69,14 @@ units::ampere_t ElevatorSim::GetCurrentDraw() const {
velocity / m_drumRadius * m_gearing * 1_rad;
// Perform calculation and return.
return m_motor.Current(motorVelocity, units::volt_t{m_u(0)}) *
return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}
void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
}
Eigen::Matrix<double, 2, 1> ElevatorSim::UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
@@ -72,7 +90,8 @@ Eigen::Matrix<double, 2, 1> ElevatorSim::UpdateX(
// Check for collision after updating x-hat.
if (HasHitLowerLimit(updatedXhat)) {
return MakeMatrix<2, 1>(m_minHeight.to<double>(), 0.0);
} else if (HasHitUpperLimit(updatedXhat)) {
}
if (HasHitUpperLimit(updatedXhat)) {
return MakeMatrix<2, 1>(m_maxHeight.to<double>(), 0.0);
}
return updatedXhat;

20
wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
View File

@@ -15,27 +15,31 @@ using namespace frc;
using namespace frc::sim;
FlywheelSim::FlywheelSim(const LinearSystem<1, 1, 1>& plant,
const DCMotor& gearbox, double gearing, bool addNoise,
const DCMotor& gearbox, double gearing,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim<1, 1, 1>(plant, addNoise, measurementStdDevs),
m_motor(gearbox),
: LinearSystemSim<1, 1, 1>(plant, measurementStdDevs),
m_gearbox(gearbox),
m_gearing(gearing) {}
FlywheelSim::FlywheelSim(const DCMotor& gearbox, double gearing,
units::kilogram_square_meter_t moi, bool addNoise,
units::kilogram_square_meter_t moi,
const std::array<double, 1>& measurementStdDevs)
: FlywheelSim(LinearSystemId::FlywheelSystem(gearbox, moi, gearing),
gearbox, gearing, addNoise, measurementStdDevs) {}
gearbox, gearing, measurementStdDevs) {}
units::radians_per_second_t FlywheelSim::GetAngularVelocity() const {
return units::radians_per_second_t{Y(0)};
return units::radians_per_second_t{GetOutput(0)};
}
units::ampere_t FlywheelSim::GetCurrentDraw() const {
// I = V / R - omega / (Kv * R)
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor
// is spinning 10x faster than the output.
return m_motor.Current(GetAngularVelocity() * m_gearing,
units::volt_t{m_u(0)}) *
return m_gearbox.Current(GetAngularVelocity() * m_gearing,
units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}
void FlywheelSim::SetInputVoltage(units::volt_t voltage) {
SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
}

43
wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
View File

@@ -19,38 +19,28 @@ using namespace frc;
using namespace frc::sim;
SingleJointedArmSim::SingleJointedArmSim(
const LinearSystem<2, 1, 1>& system, const DCMotor motor, double G,
units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool addNoise,
const std::array<double, 1>& measurementStdDevs, bool simulateGravity)
: LinearSystemSim<2, 1, 1>(system, addNoise, measurementStdDevs),
const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, units::kilogram_t mass, bool simulateGravity,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim<2, 1, 1>(system, measurementStdDevs),
m_r(armLength),
m_minAngle(minAngle),
m_maxAngle(maxAngle),
m_mass(mass),
m_motor(motor),
m_gearing(G),
m_gearbox(gearbox),
m_gearing(gearing),
m_simulateGravity(simulateGravity) {}
SingleJointedArmSim::SingleJointedArmSim(
const DCMotor& motor, units::kilogram_square_meter_t J, double G,
units::kilogram_t mass, units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool addNoise,
const std::array<double, 1>& measurementStdDevs, bool simulateGravity)
: SingleJointedArmSim(LinearSystemId::SingleJointedArmSystem(motor, J, G),
motor, G, mass, armLength, minAngle, maxAngle,
addNoise, measurementStdDevs, simulateGravity) {}
SingleJointedArmSim::SingleJointedArmSim(
const DCMotor& motor, double G, units::kilogram_t mass,
const DCMotor& gearbox, double gearing, units::kilogram_square_meter_t moi,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool addNoise,
const std::array<double, 1>& measurementStdDevs, bool simulateGravity)
units::radian_t maxAngle, units::kilogram_t mass, bool simulateGravity,
const std::array<double, 1>& measurementStdDevs)
: SingleJointedArmSim(
LinearSystemId::SingleJointedArmSystem(
motor, 1.0 / 3.0 * mass * armLength * armLength, G),
motor, G, mass, armLength, minAngle, maxAngle, addNoise,
measurementStdDevs, simulateGravity) {}
LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing),
gearbox, gearing, armLength, minAngle, maxAngle, mass,
simulateGravity, measurementStdDevs) {}
bool SingleJointedArmSim::HasHitLowerLimit(
const Eigen::Matrix<double, 2, 1>& x) const {
@@ -74,10 +64,14 @@ units::ampere_t SingleJointedArmSim::GetCurrentDraw() const {
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor
// is spinning 10x faster than the output
units::radians_per_second_t motorVelocity{m_x(1) * m_gearing};
return m_motor.Current(motorVelocity, units::volt_t{m_u(0)}) *
return m_gearbox.Current(motorVelocity, units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}
void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) {
SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
}
Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
const Eigen::Matrix<double, 2, 1>& currentXhat,
const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) {
@@ -100,7 +94,6 @@ Eigen::Matrix<double, 2, 1> SingleJointedArmSim::UpdateX(
(m_mass * m_r * m_r) * std::cos(x(0)))
.template to<double>());
}
return xdot;
},
currentXhat, u, dt);