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

[wpimath] Add full state support to LinearSystemId functions (#6554)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
This commit is contained in:
Nicholas Armstrong
2024-05-15 09:23:22 -04:00
committed by GitHub
parent 7fbbecb5b7
commit 7fc17811fa
29 changed files with 343 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
View File

@@ -41,7 +41,7 @@ units::ampere_t DCMotorSim::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_gearbox.Current(GetAngularVelocity() * m_gearing,
return m_gearbox.Current(units::radians_per_second_t{m_x(1)} * m_gearing,
units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}

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

@@ -12,11 +12,11 @@
using namespace frc;
using namespace frc::sim;
ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 1>& plant,
ElevatorSim::ElevatorSim(const LinearSystem<2, 1, 2>& plant,
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: LinearSystemSim(plant, measurementStdDevs),
m_gearbox(gearbox),
m_minHeight(minHeight),
@@ -30,7 +30,7 @@ ElevatorSim::ElevatorSim(const DCMotor& gearbox, double gearing,
units::meter_t drumRadius, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: ElevatorSim(LinearSystemId::ElevatorSystem(gearbox, carriageMass,
drumRadius, gearing),
gearbox, minHeight, maxHeight, simulateGravity,
@@ -44,7 +44,7 @@ ElevatorSim::ElevatorSim(decltype(1_V / Velocity_t<Distance>(1)) kV,
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: ElevatorSim(LinearSystemId::IdentifyPositionSystem(kV, kA), gearbox,
minHeight, maxHeight, simulateGravity, startingHeight,
measurementStdDevs) {}

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

@@ -36,7 +36,7 @@ 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_gearbox.Current(GetAngularVelocity() * m_gearing,
return m_gearbox.Current(units::radians_per_second_t{m_x(0)} * m_gearing,
units::volt_t{m_u(0)}) *
wpi::sgn(m_u(0));
}

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

@@ -16,12 +16,12 @@ using namespace frc;
using namespace frc::sim;
SingleJointedArmSim::SingleJointedArmSim(
const LinearSystem<2, 1, 1>& system, const DCMotor& gearbox, double gearing,
const LinearSystem<2, 1, 2>& system, const DCMotor& gearbox, double gearing,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs)
: LinearSystemSim<2, 1, 1>(system, measurementStdDevs),
const std::array<double, 2>& measurementStdDevs)
: LinearSystemSim<2, 1, 2>(system, measurementStdDevs),
m_armLen(armLength),
m_minAngle(minAngle),
m_maxAngle(maxAngle),
@@ -36,7 +36,7 @@ SingleJointedArmSim::SingleJointedArmSim(
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs)
const std::array<double, 2>& measurementStdDevs)
: SingleJointedArmSim(
LinearSystemId::SingleJointedArmSystem(gearbox, moi, gearing),
gearbox, gearing, armLength, minAngle, maxAngle, simulateGravity,

10
wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
View File

@@ -17,7 +17,7 @@ namespace frc::sim {
/**
* Represents a simulated elevator mechanism.
*/
class ElevatorSim : public LinearSystemSim<2, 1, 1> {
class ElevatorSim : public LinearSystemSim<2, 1, 2> {
public:
template <typename Distance>
using Velocity_t = units::unit_t<
@@ -42,10 +42,10 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
* @param startingHeight The starting height of the elevator.
* @param measurementStdDevs The standard deviation of the measurements.
*/
ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox,
ElevatorSim(const LinearSystem<2, 1, 2>& plant, const DCMotor& gearbox,
units::meter_t minHeight, units::meter_t maxHeight,
bool simulateGravity, units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});
const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
/**
* Constructs a simulated elevator mechanism.
@@ -67,7 +67,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
units::kilogram_t carriageMass, units::meter_t drumRadius,
units::meter_t minHeight, units::meter_t maxHeight,
bool simulateGravity, units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});
const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
/**
* Constructs a simulated elevator mechanism.
@@ -90,7 +90,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> {
const DCMotor& gearbox, units::meter_t minHeight,
units::meter_t maxHeight, bool simulateGravity,
units::meter_t startingHeight,
const std::array<double, 1>& measurementStdDevs = {0.0});
const std::array<double, 2>& measurementStdDevs = {0.0, 0.0});
using LinearSystemSim::SetState;
/**

10
wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
View File

@@ -18,7 +18,7 @@ namespace frc::sim {
/**
* Represents a simulated arm mechanism.
*/
class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
class SingleJointedArmSim : public LinearSystemSim<2, 1, 2> {
public:
/**
* Creates a simulated arm mechanism.
@@ -36,12 +36,13 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
* @param startingAngle The initial position of the arm.
* @param measurementStdDevs The standard deviations of the measurements.
*/
SingleJointedArmSim(const LinearSystem<2, 1, 1>& system,
SingleJointedArmSim(const LinearSystem<2, 1, 2>& system,
const DCMotor& gearbox, double gearing,
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs = {0.0});
const std::array<double, 2>& measurementStdDevs = {0.0,
0.0});
/**
* Creates a simulated arm mechanism.
*
@@ -62,7 +63,8 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> {
units::meter_t armLength, units::radian_t minAngle,
units::radian_t maxAngle, bool simulateGravity,
units::radian_t startingAngle,
const std::array<double, 1>& measurementStdDevs = {0.0});
const std::array<double, 2>& measurementStdDevs = {0.0,
0.0});
using LinearSystemSim::SetState;