diff --git a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp index 500f5de695..89c09cb9f4 100644 --- a/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp @@ -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& 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& 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& 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& x) const { return x(0) > m_maxHeight.to(); } -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())); +} + Eigen::Matrix ElevatorSim::UpdateX( const Eigen::Matrix& currentXhat, const Eigen::Matrix& u, units::second_t dt) { @@ -72,7 +90,8 @@ Eigen::Matrix ElevatorSim::UpdateX( // Check for collision after updating x-hat. if (HasHitLowerLimit(updatedXhat)) { return MakeMatrix<2, 1>(m_minHeight.to(), 0.0); - } else if (HasHitUpperLimit(updatedXhat)) { + } + if (HasHitUpperLimit(updatedXhat)) { return MakeMatrix<2, 1>(m_maxHeight.to(), 0.0); } return updatedXhat; diff --git a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp index 8748667d9b..65fc3c8257 100644 --- a/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp @@ -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& 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& 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())); +} diff --git a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp index e89affa4f0..b33a19ec82 100644 --- a/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp @@ -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& 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& 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& 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& measurementStdDevs, bool simulateGravity) + units::radian_t maxAngle, units::kilogram_t mass, bool simulateGravity, + const std::array& 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& 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())); +} + Eigen::Matrix SingleJointedArmSim::UpdateX( const Eigen::Matrix& currentXhat, const Eigen::Matrix& u, units::second_t dt) { @@ -100,7 +94,6 @@ Eigen::Matrix SingleJointedArmSim::UpdateX( (m_mass * m_r * m_r) * std::cos(x(0))) .template to()); } - return xdot; }, currentXhat, u, dt); diff --git a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h index 5c32392665..9b7069f2e5 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h @@ -25,24 +25,40 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { /** * Constructs a simulated elevator mechanism. * - * @param gearbox The type of and number of motors in your elevator - * gearbox. - * @param carriageMass The mass of the elevator carriage. - * @param gearing The gearing of the elevator (numbers greater than - * 1 represent reductions). - * @param drumRadius The radius of the drum that your cable is wrapped - * around. + * @param plant The linear system that represents the elevator. + * @param gearbox The type of and number of motors in your + * elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater + * than 1 represent reductions). + * @param drumRadius The radius of the drum that your cable is + * wrapped around. * @param minHeight The minimum allowed height of the elevator. * @param maxHeight The maximum allowed height of the elevator. - * @param addNoise Whether the sim should automatically add some - * encoder noise. - * @param measurementStdDevs The standard deviation of the measurement noise. + * @param measurementStdDevs The standard deviation of the measurements. */ - ElevatorSim(const DCMotor& gearbox, units::kilogram_t carriageMass, + ElevatorSim(const LinearSystem<2, 1, 1>& plant, const DCMotor& gearbox, double gearing, units::meter_t drumRadius, units::meter_t minHeight, units::meter_t maxHeight, - bool addNoise = false, - const std::array& m_measurementStdDevs = {0.0}); + const std::array& measurementStdDevs = {0.0}); + + /** + * Constructs a simulated elevator mechanism. + * + * @param gearbox The type of and number of motors in your + * elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater + * than 1 represent reductions). + * @param carriageMass The mass of the elevator carriage. + * @param drumRadius The radius of the drum that your cable is + * wrapped around. + * @param minHeight The minimum allowed height of the elevator. + * @param maxHeight The maximum allowed height of the elevator. + * @param measurementStdDevs The standard deviation of the measurements. + */ + 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& measurementStdDevs = {0.0}); /** * Returns whether the elevator has hit the lower limit. @@ -56,7 +72,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { * Returns whether the elevator has hit the upper limit. * * @param x The current elevator state. - * @return Whether the elevator has hit the uppwer limit. + * @return Whether the elevator has hit the upper limit. */ bool HasHitUpperLimit(const Eigen::Matrix& x) const; @@ -81,6 +97,13 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { */ units::ampere_t GetCurrentDraw() const override; + /** + * Sets the input voltage for the elevator. + * + * @param voltage The input voltage. + */ + void SetInputVoltage(units::volt_t voltage); + protected: /** * Updates the state estimate of the elevator. @@ -94,7 +117,7 @@ class ElevatorSim : public LinearSystemSim<2, 1, 1> { const Eigen::Matrix& u, units::second_t dt) override; private: - DCMotor m_motor; + DCMotor m_gearbox; units::meter_t m_drumRadius; units::meter_t m_minHeight; units::meter_t m_maxHeight; diff --git a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h index bc1748a38d..1c47c458d9 100644 --- a/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h @@ -28,12 +28,10 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> { * gearbox. * @param gearing The gearing of the flywheel (numbers greater than * 1 represent reductions). - * @param addNoise Whether the sim should automatically add some - * encoder noise. * @param measurementStdDevs The standard deviation of the measurement noise. */ FlywheelSim(const LinearSystem<1, 1, 1>& plant, const DCMotor& gearbox, - double gearing, bool addNoise = false, + double gearing, const std::array& measurementStdDevs = {0.0}); /** @@ -44,12 +42,10 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> { * @param gearing The gearing of the flywheel (numbers greater than * 1 represent reductions). * @param moi The moment of inertia of the flywheel. - * @param addNoise Whether the sim should automatically add some - * encoder noise. * @param measurementStdDevs The standard deviation of the measurement noise. */ FlywheelSim(const DCMotor& gearbox, double gearing, - units::kilogram_square_meter_t moi, bool addNoise = false, + units::kilogram_square_meter_t moi, const std::array& measurementStdDevs = {0.0}); /** @@ -66,8 +62,15 @@ class FlywheelSim : public LinearSystemSim<1, 1, 1> { */ units::ampere_t GetCurrentDraw() const override; + /** + * Sets the input voltage for the flywheel. + * + * @param voltage The input voltage. + */ + void SetInputVoltage(units::volt_t voltage); + private: - DCMotor m_motor; + DCMotor m_gearbox; double m_gearing; }; } // namespace frc::sim diff --git a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h index e24d2ad9cc..4f3c65876f 100644 --- a/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h @@ -18,7 +18,16 @@ namespace frc::sim { /** - * Represents a simulated generic linear system. + * This class helps simulate linear systems. To use this class, do the following + * in the simulationPeriodic() method. + * + * Call the SetInput() method with the inputs to your system (generally + * voltage). Call the Update() method to update the simulation. Set simulated + * sensor readings with the simulated positions in the GetOutput() method. + * + * @tparam States The number of states of the system. + * @tparam Inputs The number of inputs to the system. + * @tparam Outputs The number of outputs of the system. */ template class LinearSystemSim { @@ -27,53 +36,19 @@ class LinearSystemSim { * Creates a simulated generic linear system. * * @param system The system to simulate. - * @param addNoise Whether the sim should automatically add some - * measurement noise. - * @param measurementStdDevs The standard deviations of the measurement noise. + * @param measurementStdDevs The standard deviations of the measurements. */ LinearSystemSim(const LinearSystem& system, - bool addNoise = false, - const std::array& measurementStdDevs = {0.0}) - : m_plant(system), - m_shouldAddNoise(addNoise), - m_measurementStdDevs(measurementStdDevs) { + const std::array& measurementStdDevs = + std::array{}) + : m_plant(system), m_measurementStdDevs(measurementStdDevs) { m_x = Eigen::Matrix::Zero(); m_y = Eigen::Matrix::Zero(); m_u = Eigen::Matrix::Zero(); } /** - * Returns whether the sim should add noise to the measurements. - * - * @return Whether the sim should add noise to the measurements. - */ - bool ShouldAddNoise() const { return m_shouldAddNoise; } - - /** - * Returns the current output of the plant. - * - * @return The current output of the plant. - */ - const Eigen::Matrix& Y() const { return m_y; } - - /** - * Returns an element of the current output of the plant. - * - * @param row The row to return. - */ - double Y(int i) const { return m_y(i); } - - /** - * Sets whether the sim should add noise to measurements. - * - * @param shouldAddNoise Whether the sim should add noise to measurements. - */ - void SetShouldAddNoise(bool shouldAddNoise) { - m_shouldAddNoise = shouldAddNoise; - } - - /** - * Updates the linear system sim. + * Updates the simulation. * * @param dt The time between updates. */ @@ -85,12 +60,27 @@ class LinearSystemSim { // y = Cx + Du m_y = m_plant.CalculateY(m_x, m_u); - // Add noise if needed. - if (m_shouldAddNoise) { - m_y += frc::MakeWhiteNoiseVector(m_measurementStdDevs); - } + // Add noise. If the user did not pass a noise vector to the + // constructor, then this method will not do anything because + // the standard deviations default to zero. + m_y += frc::MakeWhiteNoiseVector(m_measurementStdDevs); } + /** + * Returns the current output of the plant. + * + * @return The current output of the plant. + */ + const Eigen::Matrix& GetOutput() const { return m_y; } + + /** + * Returns an element of the current output of the plant. + * + * @param row The row to return. + * @return An element of the current output of the plant. + */ + double GetOutput(int row) const { return m_y(row); } + /** * Sets the system inputs (usually voltages). * @@ -98,13 +88,31 @@ class LinearSystemSim { */ void SetInput(const Eigen::Matrix& u) { m_u = u; } + /* + * Sets the system inputs. + * + * @param row The row in the input matrix to set. + * @param value The value to set the row to. + */ + void SetInput(int row, double value) { m_u(row, 0) = value; } + /** * Sets the system state. * - * @param state The state. + * @param state The new state. */ void SetState(const Eigen::Matrix& state) { m_x = state; } + /** + * Returns the current drawn by this simulated system. Override this method to + * add a custom current calculation. + * + * @return The current drawn by this simulated mechanism. + */ + virtual units::ampere_t GetCurrentDraw() const { + return units::ampere_t(0.0); + } + protected: /** * Updates the state estimate of the system. @@ -119,12 +127,7 @@ class LinearSystemSim { return m_plant.CalculateX(currentXhat, u, dt); } - virtual units::ampere_t GetCurrentDraw() const { - return units::ampere_t(0.0); - } - LinearSystem m_plant; - bool m_shouldAddNoise; Eigen::Matrix m_x; Eigen::Matrix m_y; diff --git a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h index 8f6c315d3b..f8de822967 100644 --- a/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h +++ b/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h @@ -27,69 +27,43 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { * Creates a simulated arm mechanism. * * @param system The system representing this arm. - * @param motor The type and number of motors on the arm gearbox. - * @param G The gear ratio of the arm (numbers greater than 1 + * @param gearbox The type and number of motors on the arm gearbox. + * @param gearing The gear ratio of the arm (numbers greater than 1 * represent reductions). - * @param mass The mass of the arm. * @param armLength The length of the arm. - * @param minAngle The minimum allowed angle for the arm. - * @param maxAngle The maximum allowed angle for the arm. - * @param addNoise Whether the sim should automatically add some - * encoder noise. - * @param measurementStdDevs The standard deviation of the measurement noise. - * @param simulateGravity If the affects of gravity should be simulated. + * @param minAngle The minimum angle that the arm is capable of. + * @param maxAngle The maximum angle that the arm is capable of. + * @param mass The mass of the arm. + * @param measurementStdDevs The standard deviations of the measurements. + * @param simulateGravity Whether gravity should be simulated or not. */ - SingleJointedArmSim(const LinearSystem<2, 1, 1>& system, const DCMotor motor, - double G, units::kilogram_t mass, + SingleJointedArmSim(const LinearSystem<2, 1, 1>& system, + const DCMotor& gearbox, double gearing, units::meter_t armLength, units::radian_t minAngle, - units::radian_t maxAngle, bool addNoise, - const std::array& measurementStdDevs, - bool simulateGravity); + units::radian_t maxAngle, units::kilogram_t mass, + bool simulateGravity, + const std::array& measurementStdDevs = {0.0}); /** * Creates a simulated arm mechanism. * - * @param motor The type and number of motors on the arm gearbox. - * @param J The moment of inertia of the arm. - * @param G The gear ratio of the arm (numbers greater than 1 + * @param gearbox The type and number of motors on the arm gearbox. + * @param gearing The gear ratio of the arm (numbers greater than 1 * represent reductions). - * @param mass The mass of the arm. + * @param moi The moment of inertia of the arm. This can be + * calculated from CAD software. * @param armLength The length of the arm. - * @param minAngle The minimum allowed angle for the arm. - * @param maxAngle The maximum allowed angle for the arm. - * @param addNoise Whether the sim should automatically add some - * encoder noise. - * @param measurementStdDevs The standard deviation of the measurement noise. - * @param simulateGravity If the affects of gravity should be simulated. - */ - 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& measurementStdDevs, - bool simulateGravity); - - /** - * Creates a simulated arm mechanism. - * - * @param motor The type and number of motors on the arm gearbox. - * @param G The gear ratio of the arm (numbers greater than 1 - * represent reductions). + * @param minAngle The minimum angle that the arm is capable of. + * @param maxAngle The maximum angle that the arm is capable of. * @param mass The mass of the arm. - * @param armLength The length of the arm. - * @param minAngle The minimum allowed angle for the arm. This is - * measured from horizontal, with straight out being 0. - * @param maxAngle The maximum allowed angle for the arm. This is - * measured from horizontal, with straight out being 0. - * @param addNoise Whether the sim should automatically add some - * encoder noise. * @param measurementStdDevs The standard deviation of the measurement noise. - * @param simulateGravity If the affects of gravity should be simulated. + * @param simulateGravity Whether gravity should be simulated or not. */ - SingleJointedArmSim(const DCMotor& motor, double G, units::kilogram_t mass, + SingleJointedArmSim(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& measurementStdDevs, - bool simulateGravity); + units::radian_t maxAngle, units::kilogram_t mass, + bool simulateGravity, + const std::array& measurementStdDevs = {0.0}); /** * Returns whether the arm has hit the lower limit. @@ -121,8 +95,34 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { */ units::radians_per_second_t GetVelocity() const; + /** + * Returns the arm current draw. + * + * @return The arm current draw. + */ units::ampere_t GetCurrentDraw() const override; + /** + * Sets the input voltage for the elevator. + * + * @param voltage The input voltage. + */ + void SetInputVoltage(units::volt_t voltage); + + /** + * Calculates a rough estimate of the moment of inertia of an arm given its + * length and mass. + * + * @param length The length of the arm. + * @param mass The mass of the arm. + * + * @return The calculated moment of inertia. + */ + static constexpr units::kilogram_square_meter_t EstimateMOI( + units::meter_t length, units::kilogram_t mass) { + return 1.0 / 3.0 * mass * length * length; + } + protected: /** * Updates the state estimate of the arm. @@ -140,7 +140,7 @@ class SingleJointedArmSim : public LinearSystemSim<2, 1, 1> { units::radian_t m_minAngle; units::radian_t m_maxAngle; units::kilogram_t m_mass; - const DCMotor m_motor; + const DCMotor m_gearbox; double m_gearing; bool m_simulateGravity; }; diff --git a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp index 9532a04b08..05b0dc9415 100644 --- a/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp @@ -21,9 +21,9 @@ #include "gtest/gtest.h" TEST(ElevatorSim, StateSpaceSim) { - frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 8_kg, 13.67, + frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 3_m, - true, {0.01}); + {0.01}); frc2::PIDController controller(10, 0.0, 0.0); frc::PWMVictorSPX motor(0); @@ -40,7 +40,7 @@ TEST(ElevatorSim, StateSpaceSim) { sim.SetInput(u); sim.Update(20_ms); - const auto& y = sim.Y(); + const auto& y = sim.GetOutput(); encoderSim.SetDistance(y(0)); } @@ -48,9 +48,9 @@ TEST(ElevatorSim, StateSpaceSim) { } TEST(ElevatorSim, MinMax) { - frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 8_kg, 13.67, + frc::sim::ElevatorSim sim(frc::DCMotor::Vex775Pro(4), 14.67, 8_kg, units::meter_t(0.75 * 25.4 / 1000.0), 0_m, 1_m, - true, {0.01}); + {0.01}); for (size_t i = 0; i < 100; ++i) { sim.SetInput(frc::MakeMatrix<1, 1>(0.0)); sim.Update(20_ms); diff --git a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp index 12bbfdbded..a553985b75 100644 --- a/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp @@ -11,9 +11,8 @@ #include "gtest/gtest.h" TEST(SingleJointedArmTest, Disabled) { - frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 10_kg, - 9.5_in, -180_deg, 0_deg, false, {0.0}, - true); + frc::sim::SingleJointedArmSim sim(frc::DCMotor::Vex775Pro(2), 100, 3_kg_sq_m, + 9.5_in, -180_deg, 0_deg, 10_lb, true); sim.SetState(frc::MakeMatrix<2, 1>(0.0, 0.0)); for (size_t i = 0; i < 12 / 0.02; ++i) { diff --git a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp index ce53c0266e..116ee1bca9 100644 --- a/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp @@ -54,9 +54,15 @@ class Robot : public frc::TimedRobot { // 30in overall arm length, range of motion nin [-180, 0] degrees, and noise // with a standard deviation of 0.5 degrees. frc::sim::SingleJointedArmSim m_armSim{ - m_armGearbox, 100.0, 5_kg, 30_in, - -180_deg, 0_deg, true, {(0.5_deg).to()}, - true}; + m_armGearbox, + 100.0, + frc::sim::SingleJointedArmSim::EstimateMOI(30_in, 5_kg), + 30_in, + -180_deg, + 0_deg, + 5_kg, + true, + {(0.5_deg).to()}}; frc::sim::EncoderSim m_encoderSim{m_encoder}; public: diff --git a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp index 9c36ee8817..b342639b0b 100644 --- a/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp @@ -56,12 +56,11 @@ class Robot : public frc::TimedRobot { // Simulation classes help us simulate what's going on, including gravity. frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox, - kCarriageMass, kElevatorGearing, + kCarriageMass, kElevatorDrumRadius, kMinElevatorHeight, kMaxElevatorHeight, - true, {0.01}}; frc::sim::EncoderSim m_encoderSim{m_encoder}; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java index fa16a302d9..161008950c 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java @@ -16,106 +16,186 @@ import edu.wpi.first.wpiutil.math.VecBuilder; import edu.wpi.first.wpiutil.math.numbers.N1; import edu.wpi.first.wpiutil.math.numbers.N2; +/** + * Represents a simulated elevator mechanism. + */ public class ElevatorSim extends LinearSystemSim { + // Gearbox for the elevator. + private final DCMotor m_gearbox; - private final DCMotor m_motor; - private final double m_drumRadiusMeters; - private final double m_minHeightMeters; - private final double m_maxHeightMeters; + // Gearing between the motors and the output. private final double m_gearing; + // The radius of the drum that the elevator spool is wrapped around. + private final double m_drumRadius; + + // The min allowable height for the elevator. + private final double m_minHeight; + + // The max allowable height for the elevator. + private final double m_maxHeight; + /** - * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate - * * the state of the system. In simulationPeriodic, users should first set inputs from motors, update - * * the simulation and write simulated outputs to sensors. + * Creates a simulated elevator mechanism. * - * @param elevatorGearbox The {@link DCMotor} representing the elevator motor. - * @param carriageMassKg The mass of the carriage. - * @param drumRadiusMeters The radius of the drum driving the elevator. - * @param gearingReduction The reduction between motor and drum, as output over input. - * In most cases this is greater than one. - * @param m_measurementStdDevs Standard deviations for position noise. Can be null - * if no added noise is desired. + * @param plant The linear system that represents the elevator. + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). + * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. */ - public ElevatorSim(DCMotor elevatorGearbox, double carriageMassKg, - double drumRadiusMeters, double gearingReduction, - double minHeightMeters, double maxHeightMeters, - Matrix m_measurementStdDevs) { - super(LinearSystemId.createElevatorSystem(elevatorGearbox, carriageMassKg, drumRadiusMeters, - gearingReduction), m_measurementStdDevs); - this.m_motor = elevatorGearbox; - this.m_gearing = gearingReduction; - this.m_drumRadiusMeters = drumRadiusMeters; - this.m_minHeightMeters = minHeightMeters; - this.m_maxHeightMeters = maxHeightMeters; + public ElevatorSim(LinearSystem plant, DCMotor gearbox, double gearing, + double drumRadiusMeters, double minHeightMeters, double maxHeightMeters) { + this(plant, gearbox, gearing, drumRadiusMeters, minHeightMeters, maxHeightMeters, null); } /** - * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate - * the state of the system. In simulationPeriodic, users should first set inputs from motors, update - * the simulation and write simulated outputs to sensors. + * Creates a simulated elevator mechanism. * - * @param elevatorPlant The elevator system being controlled. This system can be created - * with {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double, double, double)} - * or {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#identifyPositionSystem(double, double)}. - * @param m_measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false. + * @param plant The linear system that represents the elevator. + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). + * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param measurementStdDevs The standard deviations of the measurements. */ - public ElevatorSim(LinearSystem elevatorPlant, - Matrix m_measurementStdDevs, DCMotor elevatorGearbox, - double gearingReduction, double drumRadiusMeters, - double minHeightMeters, double maxHeightMeters) { - super(elevatorPlant, m_measurementStdDevs); - this.m_motor = elevatorGearbox; - this.m_gearing = gearingReduction; - this.m_drumRadiusMeters = drumRadiusMeters; - this.m_minHeightMeters = minHeightMeters; - this.m_maxHeightMeters = maxHeightMeters; + public ElevatorSim(LinearSystem plant, DCMotor gearbox, double gearing, + double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, + Matrix measurementStdDevs) { + super(plant, measurementStdDevs); + m_gearbox = gearbox; + m_gearing = gearing; + m_drumRadius = drumRadiusMeters; + m_minHeight = minHeightMeters; + m_maxHeight = maxHeightMeters; } + /** + * Creates a simulated elevator mechanism. + * + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). + * @param carriageMassKg The mass of the elevator carriage. + * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + */ + public ElevatorSim(DCMotor gearbox, double gearing, double carriageMassKg, + double drumRadiusMeters, double minHeightMeters, double maxHeightMeters) { + this(gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters, + null); + } + + /** + * Creates a simulated elevator mechanism. + * + * @param gearbox The type of and number of motors in the elevator gearbox. + * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions). + * @param carriageMassKg The mass of the elevator carriage. + * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around. + * @param minHeightMeters The min allowable height of the elevator. + * @param maxHeightMeters The max allowable height of the elevator. + * @param measurementStdDevs The standard deviations of the measurements. + */ + public ElevatorSim(DCMotor gearbox, double gearing, double carriageMassKg, + double drumRadiusMeters, double minHeightMeters, double maxHeightMeters, + Matrix measurementStdDevs) { + super(LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing), + measurementStdDevs); + m_gearbox = gearbox; + m_gearing = gearing; + m_drumRadius = drumRadiusMeters; + m_minHeight = minHeightMeters; + m_maxHeight = maxHeightMeters; + } + + /** + * Returns whether the elevator has hit the lower limit. + * + * @param x The current elevator state. + * @return Whether the elevator has hit the lower limit. + */ public boolean hasHitLowerLimit(Matrix x) { - return x.get(0, 0) < this.m_minHeightMeters; + return x.get(0, 0) < this.m_minHeight; } + /** + * Returns whether the elevator has hit the upper limit. + * + * @param x The current elevator state. + * @return Whether the elevator has hit the upper limit. + */ public boolean hasHitUpperLimit(Matrix x) { - return x.get(0, 0) > this.m_maxHeightMeters; - } - - @Override - protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { - var updatedXhat = RungeKutta.rungeKutta( - (x, u_) -> (m_plant.getA().times(x)).plus(m_plant.getB().times(u_)).plus(VecBuilder.fill(0, -9.8)), - currentXhat, u, dtSeconds); - - // We check for collision after updating xhat - if (hasHitLowerLimit(updatedXhat)) { - return VecBuilder.fill(m_minHeightMeters, 0); - } else if (hasHitUpperLimit(updatedXhat)) { - return VecBuilder.fill(m_maxHeightMeters, 0); - } - - return updatedXhat; - } - - public void setInputVoltage(double volts) { - setInput(volts); + return x.get(0, 0) > this.m_maxHeight; } + /** + * Returns the position of the elevator. + * + * @return The position of the elevator. + */ public double getPositionMeters() { return getOutput(0); } + /** + * Returns the velocity of the elevator. + * + * @return The velocity of the elevator. + */ public double getVelocityMetersPerSecond() { return m_x.get(0, 1); } + /** + * Returns the elevator current draw. + * + * @return The elevator current draw. + */ @Override public double getCurrentDrawAmps() { // 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 // v = r w, so w = v/r - double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadiusMeters * m_gearing; - var appliedVoltage = getInput(0); - return m_motor.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage); + double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadius * m_gearing; + var appliedVoltage = m_u.get(0, 0); + return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage); + } + + /** + * Sets the input voltage for the elevator. + * + * @param volts The input voltage. + */ + public void setInputVoltage(double volts) { + setInput(volts); + } + + /** + * Updates the state of the elevator. + * + * @param currentXhat The current state estimate. + * @param u The system inputs (voltage). + * @param dtSeconds The time difference between controller updates. + */ + @Override + protected Matrix updateX(Matrix currentXhat, + Matrix u, double dtSeconds) { + // Calculate updated x-hat from Runge-Kutta. + var updatedXhat = RungeKutta.rungeKutta( + (x, u_) -> (m_plant.getA().times(x)).plus(m_plant.getB().times(u_)) + .plus(VecBuilder.fill(0, -9.8)), currentXhat, u, dtSeconds); + + // We check for collisions after updating x-hat. + if (hasHitLowerLimit(updatedXhat)) { + return VecBuilder.fill(m_minHeight, 0); + } + if (hasHitUpperLimit(updatedXhat)) { + return VecBuilder.fill(m_maxHeight, 0); + } + return updatedXhat; } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java index f3ffeff801..785faccd0e 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java @@ -14,66 +14,121 @@ import edu.wpi.first.wpilibj.util.Units; import edu.wpi.first.wpiutil.math.Matrix; import edu.wpi.first.wpiutil.math.numbers.N1; +/** + * Represents a simulated flywheel mechanism. + */ public class FlywheelSim extends LinearSystemSim { + // Gearbox for the flywheel. + private final DCMotor m_gearbox; - private final DCMotor m_motor; + // The gearing from the motors to the output. private final double m_gearing; /** - * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate - * the state of the system. In simulationPeriodic, users should first set inputs from motors, update - * the simulation and write simulated outputs to sensors. + * Creates a simulated flywheel mechanism. * - * @param flywheelPlant The flywheel system being controlled. This system can be created - * using {@link LinearSystemId#createFlywheelSystem(DCMotor, double, double)} - * or {@link LinearSystemId#identifyVelocitySystem(double, double)} - * @param flywheelGearbox The DCMotor representing the motor driving the flywheel. - * @param gearing The reduction between motor and drum, as output over input. - * In most cases this is greater than one. - * @param measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false. + * @param plant The linear system that represents the flywheel. + * @param gearbox The type of and number of motors in the flywheel gearbox. + * @param gearing The gearing of the flywheel (numbers greater than 1 + * represent reductions). */ - public FlywheelSim(LinearSystem flywheelPlant, DCMotor flywheelGearbox, - double gearing, Matrix measurementStdDevs) { - super(flywheelPlant, measurementStdDevs); - this.m_motor = flywheelGearbox; - this.m_gearing = gearing; + public FlywheelSim(LinearSystem plant, DCMotor gearbox, double gearing) { + super(plant); + m_gearbox = gearbox; + m_gearing = gearing; } /** - * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate - * the state of the system. In simulationPeriodic, users should first set inputs from motors, update - * the simulation and write simulated outputs to sensors. + * Creates a simulated flywheel mechanism. * - * @param flywheelGearbox The DCMotor representing the motor driving the flywheel. - * @param gearing The reduction between motor and drum, as output over input. - * In most cases this is greater than one. - * @param jKgMetersSquared The moment of inertia J of the flywheel. This can be - * calculated with CAD. If J is unknown, {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)} - * using FRC-Characterization's kV and kA may be better. - * @param measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false. + * @param plant The linear system that represents the flywheel. + * @param gearbox The type of and number of motors in the flywheel + * gearbox. + * @param gearing The gearing of the flywheel (numbers greater + * than 1 represent reductions). + * @param measurementStdDevs The standard deviations of the measurements. */ - public FlywheelSim(DCMotor flywheelGearbox, double gearing, - double jKgMetersSquared, Matrix measurementStdDevs) { - super(LinearSystemId.createFlywheelSystem(flywheelGearbox, jKgMetersSquared, gearing), - measurementStdDevs); - this.m_motor = flywheelGearbox; - this.m_gearing = gearing; + public FlywheelSim(LinearSystem plant, DCMotor gearbox, double gearing, + Matrix measurementStdDevs) { + super(plant, measurementStdDevs); + m_gearbox = gearbox; + m_gearing = gearing; } + /** + * Creates a simulated flywheel mechanism. + * + * @param gearbox The type of and number of motors in the flywheel gearbox. + * @param gearing The gearing of the flywheel (numbers greater than 1 + * represent reductions). + * @param jKgMetersSquared The moment of inertia of the flywheel. If this is unknown, + * use the {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)} + * constructor. + */ + public FlywheelSim(DCMotor gearbox, double gearing, double jKgMetersSquared) { + super(LinearSystemId.createFlywheelSystem(gearbox, gearing, jKgMetersSquared)); + m_gearbox = gearbox; + m_gearing = gearing; + } + + /** + * Creates a simulated flywheel mechanism. + * + * @param gearbox The type of and number of motors in the flywheel + * gearbox. + * @param gearing The gearing of the flywheel (numbers greater + * than 1 represent reductions). + * @param jKgMetersSquared The moment of inertia of the flywheel. If this is unknown, + * use the {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)} + * constructor. + * @param measurementStdDevs The standard deviations of the measurements. + */ + public FlywheelSim(DCMotor gearbox, double gearing, double jKgMetersSquared, + Matrix measurementStdDevs) { + super(LinearSystemId.createFlywheelSystem(gearbox, gearing, jKgMetersSquared), + measurementStdDevs); + m_gearbox = gearbox; + m_gearing = gearing; + } + + /** + * Returns the flywheel velocity. + * + * @return The flywheel velocity. + */ public double getAngularVelocityRadPerSec() { return getOutput(0); } + /** + * Returns the flywheel velocity in RPM. + * + * @return The flywheel velocity in RPM. + */ public double getAngularVelocityRPM() { return Units.radiansPerSecondToRotationsPerMinute(getOutput(0)); } + /** + * Returns the flywheel current draw. + * + * @return The flywheel current draw. + */ @Override public double getCurrentDrawAmps() { // I = V / R - omega / (Kv * R) // Reductions are output over input, so a reduction of 2:1 means the motor is spinning // 2x faster than the flywheel - return m_motor.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0)) + return m_gearbox.getCurrent(getAngularVelocityRadPerSec() * m_gearing, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); } + + /** + * Sets the input voltage for the flywheel. + * + * @param volts The input voltage. + */ + public void setInputVoltage(double volts) { + setInput(volts); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java index a82ea6a58f..dc69ffa33f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java @@ -7,13 +7,14 @@ package edu.wpi.first.wpilibj.simulation; +import org.ejml.MatrixDimensionException; +import org.ejml.simple.SimpleMatrix; + import edu.wpi.first.wpilibj.math.StateSpaceUtil; import edu.wpi.first.wpilibj.system.LinearSystem; import edu.wpi.first.wpiutil.math.Matrix; import edu.wpi.first.wpiutil.math.Num; import edu.wpi.first.wpiutil.math.numbers.N1; -import org.ejml.MatrixDimensionException; -import org.ejml.simple.SimpleMatrix; /** * This class helps simulate linear systems. To use this class, do the following in the @@ -23,35 +24,46 @@ import org.ejml.simple.SimpleMatrix; * *

Call {@link #update} to update the simulation. * - *

Set simulated sensor readings with the simulated positions in {@link #getOutput(int)} + *

Set simulated sensor readings with the simulated positions in {@link #getOutput()} * - * @param The number of states of the system. - * @param The number of inputs to the system. + * @param The number of states of the system. + * @param The number of inputs to the system. * @param The number of outputs of the system. */ @SuppressWarnings("ClassTypeParameterName") -public class LinearSystemSim { - +public class LinearSystemSim { + // The plant that represents the linear system. protected final LinearSystem m_plant; + // Variables for state, output, and input. @SuppressWarnings("MemberName") protected Matrix m_x; @SuppressWarnings("MemberName") protected Matrix m_y; @SuppressWarnings("MemberName") protected Matrix m_u; + + // The standard deviations of measurements, used for adding noise + // to the measurements. protected final Matrix m_measurementStdDevs; /** - * Create a SimLinearSystem. This simulator uses an {@link LinearSystem} to simulate - * the state of the system. In simulationPeriodic, users should first set inputs from motors, update - * the simulation and write simulated outputs to sensors. + * Creates a simulated generic linear system. + * + * @param system The system to simulate. + */ + public LinearSystemSim(LinearSystem system) { + this(system, null); + } + + /** + * Creates a simulated generic linear system with measurement noise. * * @param system The system being controlled. - * @param measurementStdDevs Standard deviations of measurements. Can be null if addNoise is false. + * @param measurementStdDevs Standard deviations of measurements. */ - public LinearSystemSim(LinearSystem system, Matrix measurementStdDevs) { + public LinearSystemSim(LinearSystem system, + Matrix measurementStdDevs) { this.m_plant = system; this.m_measurementStdDevs = measurementStdDevs; @@ -60,14 +72,10 @@ public class LinearSystemSim(new SimpleMatrix(system.getC().getNumRows(), 1)); } - protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { - return m_plant.calculateX(currentXhat, u, dtSeconds); - } - /** * Updates the simulation. * - * @param dtSeconds The time that's passed since the last {@link #update(double)} call. + * @param dtSeconds The time between updates. */ @SuppressWarnings("LocalVariableName") public void update(double dtSeconds) { @@ -76,67 +84,93 @@ public class LinearSystemSim getY() { - return m_y; - } - - public double getY(int row) { - return m_y.get(row, 0); - } - - public void setInput(Matrix u) { - this.m_u = u; - } - - public void setInput(int row, double value) { - m_u.set(row, 0, value); - } - - public void setInput(double... u) { - if (u.length != m_u.getNumRows()) { - throw new MatrixDimensionException("Malformed input! Got " + u.length - + " elements instead of " + m_u.getNumRows()); - } - m_u = new Matrix<>(new SimpleMatrix(m_u.getNumRows(), 1, true, u)); - } - - public double getInput(int row) { - return m_u.get(row, 0); - } - - public Matrix getInput() { - return m_u; - } - + /** + * Returns the current output of the plant. + * + * @return The current output of the plant. + */ public Matrix getOutput() { return m_y; } + /** + * Returns an element of the current output of the plant. + * + * @param row The row to return. + * @return An element of the current output of the plant. + */ public double getOutput(int row) { return m_y.get(row, 0); } + /** + * Sets the system inputs (usually voltages). + * + * @param u The system inputs. + */ + public void setInput(Matrix u) { + this.m_u = u; + } + + /** + * Sets the system inputs. + * + * @param row The row in the input matrix to set. + * @param value The value to set the row to. + */ + public void setInput(int row, double value) { + m_u.set(row, 0, value); + } + + /** + * Sets the system inputs. + * + * @param u An array of doubles that represent the inputs of the system. + */ + public void setInput(double... u) { + if (u.length != m_u.getNumRows()) { + throw new MatrixDimensionException("Malformed input! Got " + u.length + + " elements instead of " + m_u.getNumRows()); + } + m_u = new Matrix<>(new SimpleMatrix(m_u.getNumRows(), 1, true, u)); + } + /** * Sets the system state. * - * @param state The state. + * @param state The new state. */ public void setState(Matrix state) { m_x = state; } /** - * Get the current drawn by this simulated system. Override this method to add current - * calculation. + * Returns the current drawn by this simulated system. Override this method to add a custom + * current calculation. * - * @return The currently drawn current. + * @return The current drawn by this simulated mechanism. */ public double getCurrentDrawAmps() { return 0.0; } + + /** + * Updates the state estimate of the system. + * + * @param currentXhat The current state estimate. + * @param u The system inputs (usually voltage). + * @param dtSeconds The time difference between controller updates. + * @return The new state. + */ + protected Matrix updateX(Matrix currentXhat, + Matrix u, double dtSeconds) { + return m_plant.calculateX(currentXhat, u, dtSeconds); + } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java index e7568940a9..f3188a0d83 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java @@ -16,195 +16,235 @@ import edu.wpi.first.wpiutil.math.VecBuilder; import edu.wpi.first.wpiutil.math.numbers.N1; import edu.wpi.first.wpiutil.math.numbers.N2; +/** + * Represents a simulated single jointed arm mechanism. + */ public class SingleJointedArmSim extends LinearSystemSim { + // The gearbox for the arm. + private final DCMotor m_gearbox; + // The gearing between the motors and the output. + private final double m_gearing; + + // The length of the arm. @SuppressWarnings("MemberName") private final double m_r; - private final double m_maxAngleRads; - private final double m_minAngleRads; + + // The minimum angle that the arm is capable of. + private final double m_minAngle; + + // The maximum angle that the arm is capable of. + private final double m_maxAngle; + + // The mass of the arm. private final double m_armMass; - private final DCMotor m_motor; - private final double m_gearing; + + // Whether the simulator should simulate gravity. private final boolean m_simulateGravity; /** - * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate - * the state of the system. In simulationPeriodic, users should first set inputs from motors, update - * the simulation and write simulated outputs to sensors. + * Creates a simulated arm mechanism. * - * @param armSystem The arm system being controlled. - * @param motor DCMotor representing the motor driving the arm. - * @param G The gear ratio of the arm (numbers greater than 1 - * represent reductions). - * @param armMassKg The mass of the arm. - * @param armLengthMeters The distance from the pivot of the arm to its center of mass. - * This number is not the same as the overall length of the arm. - * @param minAngleRads The min angle the arm can reach, with 0 being measured from - * horizontal. The simulation will not allow motion past this. - * @param maxAngleRads The max angle the arm can reach, with 0 being measured from - * horizontal. The simulation will not allow motion past this. - * @param posMeasurementStdDev Standard deviations of noise in the arm's position measurement. - * @param simulateGravity If the affects of gravity should be simulated. + * @param plant The linear system that represents the arm. + * @param gearbox The type of and number of motors in the arm gearbox. + * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). + * @param armLengthMeters The length of the arm. + * @param minAngleRads The minimum angle that the arm is capable of. + * @param maxAngleRads The maximum angle that the arm is capable of. + * @param armMassKg The mass of the arm. + * @param simulateGravity Whether gravity should be simulated or not. */ - public SingleJointedArmSim(LinearSystem armSystem, - DCMotor motor, double G, - double armMassKg, + public SingleJointedArmSim(LinearSystem plant, DCMotor gearbox, double gearing, double armLengthMeters, double minAngleRads, double maxAngleRads, - double posMeasurementStdDev, boolean simulateGravity) { - super(armSystem, VecBuilder.fill(posMeasurementStdDev)); - this.m_armMass = armMassKg; - this.m_r = armLengthMeters; - this.m_minAngleRads = minAngleRads; - this.m_maxAngleRads = maxAngleRads; - this.m_motor = motor; - this.m_gearing = G; - this.m_simulateGravity = simulateGravity; + double armMassKg, boolean simulateGravity) { + this(plant, gearbox, gearing, armLengthMeters, minAngleRads, maxAngleRads, armMassKg, + simulateGravity, null); } /** - * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate - * the state of the system. In simulationPeriodic, users should first set inputs from motors, update - * the simulation and write simulated outputs to sensors. + * Creates a simulated arm mechanism. * - * @param motor DCMotor representing the motor driving the arm. - * @param jKgSquaredMeters The moment of inertia of the arm. - * @param G The gear ratio of the arm (numbers greater than 1 - * represent reductions). - * @param armMassKg The mass of the arm. - * @param armLengthMeters The distance from the pivot of the arm to its center of mass. - * @param minAngleRads The min angle the arm can reach, with 0 being measured from - * horizontal. The simulation will not allow motion past this. - * @param maxAngleRads The max angle the arm can reach, with 0 being measured from - * horizontal. The simulation will not allow motion past this. - * @param posMeasurementStdDev Standard deviations of noise in the arm's position measurement. - * @param simulateGravity If the affects of gravity should be simulated. + * @param plant The linear system that represents the arm. + * @param gearbox The type of and number of motors in the arm gearbox. + * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). + * @param armLengthMeters The length of the arm. + * @param minAngleRads The minimum angle that the arm is capable of. + * @param maxAngleRads The maximum angle that the arm is capable of. + * @param armMassKg The mass of the arm. + * @param simulateGravity Whether gravity should be simulated or not. + * @param measurementStdDevs The standard deviations of the measurements. */ - public SingleJointedArmSim(DCMotor motor, double jKgSquaredMeters, - double G, double armMassKg, double armLengthMeters, - double minAngleRads, double maxAngleRads, - double posMeasurementStdDev, boolean simulateGravity) { - this(LinearSystemId.createSingleJointedArmSystem(motor, jKgSquaredMeters, G), - motor, G, - armMassKg, armLengthMeters, minAngleRads, maxAngleRads, - posMeasurementStdDev, simulateGravity); + public SingleJointedArmSim(LinearSystem plant, DCMotor gearbox, double gearing, + double armLengthMeters, double minAngleRads, double maxAngleRads, + double armMassKg, boolean simulateGravity, + Matrix measurementStdDevs) { + super(plant, measurementStdDevs); + m_gearbox = gearbox; + m_gearing = gearing; + m_r = armLengthMeters; + m_minAngle = minAngleRads; + m_maxAngle = maxAngleRads; + m_armMass = armMassKg; + m_simulateGravity = simulateGravity; } /** - * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate - * the state of the system. In simulationPeriodic, users should first set inputs from motors, update - * the simulation and write simulated outputs to sensors. + * Creates a simulated arm mechanism. * - * @param motor DCMotor representing the motor driving the arm. - * @param G The gear ratio of the arm (numbers greater than 1 - * represent reductions). - * @param armMassKg The mass of the arm. - * @param armLengthMeters The distance from the pivot of the arm to its center of mass. - * @param minAngleRads The min angle the arm can reach, with 0 being measured from - * horizontal. The simulation will not allow motion past this. - * @param maxAngleRads The max angle the arm can reach, with 0 being measured from - * horizontal. The simulation will not allow motion past this. - * @param positionMeasurementStdDevs Standard deviations of noise in the arm's position measurement. - * @param simulateGravity If the affects of gravity should be simulated. + * @param gearbox The type of and number of motors in the arm gearbox. + * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). + * @param jKgMetersSquared The moment of inertia of the arm. This can be calculated from CAD software. + * @param armLengthMeters The length of the arm. + * @param minAngleRads The minimum angle that the arm is capable of. + * @param maxAngleRads The maximum angle that the arm is capable of. + * @param armMassKg The mass of the arm. + * @param simulateGravity Whether gravity should be simulated or not. */ - public SingleJointedArmSim(DCMotor motor, - double G, double armMassKg, double armLengthMeters, - double minAngleRads, double maxAngleRads, - double positionMeasurementStdDevs, boolean simulateGravity) { - this(LinearSystemId.createSingleJointedArmSystem(motor, - 1.0 / 3.0 * armMassKg * armLengthMeters * armLengthMeters, G), - motor, G, - armMassKg, armLengthMeters, minAngleRads, maxAngleRads, - positionMeasurementStdDevs, simulateGravity); + public SingleJointedArmSim(DCMotor gearbox, double gearing, double jKgMetersSquared, + double armLengthMeters, double minAngleRads, double maxAngleRads, + double armMassKg, boolean simulateGravity) { + this(gearbox, gearing, jKgMetersSquared, armLengthMeters, minAngleRads, maxAngleRads, + armMassKg, simulateGravity, null); } - /** - * Create a LinearSystemSimulator. This simulator uses an {@link LinearSystem} to simulate - * the state of the system. In simulationPeriodic, users should first set inputs from motors, update - * the simulation and write simulated outputs to sensors. + * Creates a simulated arm mechanism. * - * @param motor DCMotor representing the motor driving the arm. - * @param G The gear ratio of the arm (numbers greater than 1 - * represent reductions). - * @param armMassKg The mass of the arm. - * @param armLengthMeters The distance from the pivot of the arm to its center of mass. - * @param minAngleRads The min angle the arm can reach, with 0 being measured from - * horizontal. The simulation will not allow motion past this. - * @param maxAngleRads The max angle the arm can reach, with 0 being measured from - * horizontal. The simulation will not allow motion past this. - * @param positionMeasurementStdDevs Standard deviations of noise in the arm's position measurement. + * @param gearbox The type of and number of motors in the arm gearbox. + * @param gearing The gearing of the arm (numbers greater than 1 represent reductions). + * @param jKgMetersSquared The moment of inertia of the arm. This can be calculated from CAD software. + * @param armLengthMeters The length of the arm. + * @param minAngleRads The minimum angle that the arm is capable of. + * @param maxAngleRads The maximum angle that the arm is capable of. + * @param armMassKg The mass of the arm. + * @param simulateGravity Whether gravity should be simulated or not. + * @param measurementStdDevs The standard deviations of the measurements. */ - public SingleJointedArmSim(DCMotor motor, - double G, double armMassKg, double armLengthMeters, - double minAngleRads, double maxAngleRads, - double positionMeasurementStdDevs) { - this(LinearSystemId.createSingleJointedArmSystem(motor, - 1.0 / 3.0 * armMassKg * armLengthMeters * armLengthMeters, G), - motor, G, - armMassKg, armLengthMeters, minAngleRads, maxAngleRads, - positionMeasurementStdDevs, true); + public SingleJointedArmSim(DCMotor gearbox, double gearing, double jKgMetersSquared, + double armLengthMeters, double minAngleRads, double maxAngleRads, + double armMassKg, boolean simulateGravity, + Matrix measurementStdDevs) { + super(LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing), + measurementStdDevs); + m_gearbox = gearbox; + m_gearing = gearing; + m_r = armLengthMeters; + m_minAngle = minAngleRads; + m_maxAngle = maxAngleRads; + m_armMass = armMassKg; + m_simulateGravity = simulateGravity; } + /** + * Returns whether the arm has hit the lower limit. + * + * @param x The current arm state. + * @return Whether the arm has hit the lower limit. + */ public boolean hasHitLowerLimit(Matrix x) { - return x.get(0, 0) < this.m_minAngleRads; + return x.get(0, 0) < this.m_minAngle; } + /** + * Returns whether the arm has hit the upper limit. + * + * @param x The current arm state. + * @return Whether the arm has hit the upper limit. + */ public boolean hasHitUpperLimit(Matrix x) { - return x.get(0, 0) > this.m_maxAngleRads; - } - - @Override - protected Matrix updateX(Matrix currentXhat, Matrix u, double dtSeconds) { - /* - Horizontal case: - Torque = f * r = I * alpha - alpha = f * r / I - since f=mg, - alpha = m g r / I - - Multiply RHS by cos(theta) to account for the arm angle - - This acceleration is added to the linear system dynamics x-dot = Ax + Bu - We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I]] - */ - Matrix updatedXhat = RungeKutta.rungeKutta( - this::dynamics, - currentXhat, u, dtSeconds); - - // We check for collision after updating xhat - if (hasHitLowerLimit(updatedXhat)) { - return VecBuilder.fill(m_minAngleRads, 0); - } else if (hasHitUpperLimit(updatedXhat)) { - return VecBuilder.fill(m_maxAngleRads, 0); - } - - return updatedXhat; + return x.get(0, 0) > this.m_maxAngle; } + /** + * Returns the current arm angle. + * + * @return The current arm angle. + */ public double getAngleRads() { return m_x.get(0, 0); } + /** + * Returns the current arm velocity. + * + * @return The current arm velocity. + */ public double getVelocityRadPerSec() { return m_x.get(1, 0); } + /** + * Returns the arm current draw. + * + * @return The aram current draw. + */ @Override public double getCurrentDrawAmps() { // Reductions are greater than 1, so a reduction of 10:1 would mean the motor is // spinning 10x faster than the output var motorVelocity = m_x.get(1, 0) * m_gearing; - return m_motor.getCurrent(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); + return m_gearbox.getCurrent(motorVelocity, m_u.get(0, 0)) * Math.signum(m_u.get(0, 0)); } - private Matrix dynamics(Matrix x, Matrix u_) { - Matrix xdot = (m_plant.getA().times(x)) - .plus(m_plant.getB().times(u_)); + /** + * Sets the input voltage for the elevator. + * + * @param volts The input voltage. + */ + public void setInputVoltage(double volts) { + setInput(volts); + } - if (m_simulateGravity) { - xdot = xdot.plus(VecBuilder.fill(0, - m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) * Math.cos(x.get(0, 0)))); + /** + * Calculates a rough estimate of the moment of inertia of an arm given its + * length and mass. + * + * @param lengthMeters The length of the arm. + * @param massKg The mass of the arm. + * + * @return The calculated moment of inertia. + */ + public static double estimateMOI(double lengthMeters, double massKg) { + return 1.0 / 3.0 * massKg * lengthMeters * lengthMeters; + } + + /** + * Updates the state of the arm. + * + * @param currentXhat The current state estimate. + * @param u The system inputs (voltage). + * @param dtSeconds The time difference between controller updates. + */ + @Override + protected Matrix updateX(Matrix currentXhat, + Matrix u, double dtSeconds) { + // Horizontal case: + // Torque = F * r = I * alpha + // alpha = F * r / I + // Since F = mg, + // alpha = m * g * r / I + // Finally, multiply RHS by cos(theta) to account for the arm angle + // This acceleration is added to the linear system dynamics x-dot = Ax + Bu + // We therefore find that f(x, u) = Ax + Bu + [[0] [m * g * r / I * + // cos(theta)]] + Matrix updatedXhat = RungeKutta.rungeKutta( + (Matrix x, Matrix u_) -> { + Matrix xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_)); + if (m_simulateGravity) { + xdot = xdot.plus(VecBuilder.fill(0, + m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) * Math.cos(x.get(0, 0)))); + } + return xdot; + }, + currentXhat, u, dtSeconds); + + // We check for collision after updating xhat + if (hasHitLowerLimit(updatedXhat)) { + return VecBuilder.fill(m_minAngle, 0); } - return xdot; + if (hasHitUpperLimit(updatedXhat)) { + return VecBuilder.fill(m_maxAngle, 0); + } + return updatedXhat; } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java index e127821491..c4d78d0f3b 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java @@ -7,6 +7,8 @@ package edu.wpi.first.wpilibj.simulation; +import org.junit.jupiter.api.Test; + import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.PWMVictorSPX; import edu.wpi.first.wpilibj.RobotController; @@ -14,7 +16,6 @@ import edu.wpi.first.wpilibj.controller.PIDController; import edu.wpi.first.wpilibj.system.plant.DCMotor; import edu.wpi.first.wpilibj.system.plant.LinearSystemId; import edu.wpi.first.wpiutil.math.VecBuilder; -import org.junit.jupiter.api.Test; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -26,7 +27,7 @@ public class ElevatorSimTest { var controller = new PIDController(10, 0, 0); - var sim = new ElevatorSim(DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67, + var sim = new ElevatorSim(DCMotor.getVex775Pro(4), 14.67, 8, 0.75 * 25.4 / 1000.0, 0.0, 3.0, VecBuilder.fill(0.01)); var motor = new PWMVictorSPX(0); @@ -61,9 +62,9 @@ public class ElevatorSimTest { 0.75 * 25.4 / 1000.0, 14.67); - var sim = new ElevatorSim(plant, VecBuilder.fill(0.01), + var sim = new ElevatorSim(plant, DCMotor.getVex775Pro(4), - 14.67, 0.75 * 25.4 / 1000.0, 0.0, 1.0); + 14.67, 0.75 * 25.4 / 1000.0, 0.0, 1.0, VecBuilder.fill(0.01)); for (int i = 0; i < 100; i++) { sim.setInput(VecBuilder.fill(0)); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ArmSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java similarity index 93% rename from wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ArmSimTest.java rename to wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java index e185b3809c..f1b4bc823a 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ArmSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java @@ -7,21 +7,22 @@ package edu.wpi.first.wpilibj.simulation; +import org.junit.jupiter.api.Test; + import edu.wpi.first.wpilibj.system.plant.DCMotor; import edu.wpi.first.wpilibj.util.Units; import edu.wpi.first.wpiutil.math.VecBuilder; -import org.junit.jupiter.api.Test; import static org.junit.jupiter.api.Assertions.assertEquals; -public class ArmSimTest { +public class SingleJointedArmSimTest { SingleJointedArmSim m_sim = new SingleJointedArmSim( DCMotor.getVex775Pro(2), 100, - 10 / 2.2, + 3.0, Units.inchesToMeters(19.0 / 2.0), -Math.PI, - 0.0, 0.0); + 0.0, 10.0 / 2.2, true); @Test public void testArmDisabled() { diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java index 4eb77bbba4..c9063810dd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; import edu.wpi.first.wpilibj.system.plant.DCMotor; import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.VecBuilder; /** * This is a sample program to demonstrate the use of elevator simulation with existing code. @@ -53,11 +54,13 @@ public class Robot extends TimedRobot { // to 0 degrees (rotated straight forwards). private final SingleJointedArmSim m_armSim = new SingleJointedArmSim(m_armGearbox, m_armReduction, - m_armMass, + SingleJointedArmSim.estimateMOI(m_armLength, m_armMass), m_armLength, Units.degreesToRadians(-180), Units.degreesToRadians(0), - Units.degreesToRadians(0.5) // Add noise with a standard deviation of 0.5 degrees + m_armMass, + true, + VecBuilder.fill(Units.degreesToRadians(0.5)) // Add noise with a std-dev of 0.5 degrees ); private final EncoderSim m_encoderSim = new EncoderSim(m_encoder); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java index 22f653cfe4..600437a9e5 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java @@ -52,9 +52,9 @@ public class Robot extends TimedRobot { // Simulation classes help us simulate what's going on, including gravity. private final ElevatorSim m_elevatorSim = new ElevatorSim(m_elevatorGearbox, + kElevatorGearing, kCarriageMass, kElevatorDrumRadius, - kElevatorGearing, kMinElevatorHeight, kMaxElevatorHeight, VecBuilder.fill(0.01));