[wpilib] Add physics simulation support with state-space (#2615)

This includes physics simulation support for arms/elevator models, as well as differential drivetrains. Swerve might be added at a later date. Co-authored-by: Claudius Tewari <cttewari@gmail.com> Co-authored-by: Prateek Machiraju <prateek.machiraju@gmail.com> Co-authored-by: Tyler Veness <calcmogul@gmail.com>
2026-06-25 01:41:43 +00:00 · 2020-09-20 09:39:52 -07:00
parent 0503225928
commit b61f08d3fa
43 changed files with 3787 additions and 31 deletions
--- a/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
+++ b/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
@@ -0,0 +1,103 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <array>
+
+#include <units/length.h>
+#include <units/mass.h>
+#include <units/velocity.h>
+
+#include "frc/simulation/LinearSystemSim.h"
+#include "frc/system/plant/DCMotor.h"
+
+namespace frc::sim {
+/**
+ * Represents a simulated elevator mechanism.
+ */
+class ElevatorSim : public LinearSystemSim<2, 1, 1> {
+ public:
+  /**
+   * 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 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.
+   */
+  ElevatorSim(const DCMotor& gearbox, units::kilogram_t carriageMass,
+              double gearing, units::meter_t drumRadius,
+              units::meter_t minHeight, units::meter_t maxHeight,
+              bool addNoise = false,
+              const std::array<double, 1>& m_measurementStdDevs = {0.0});
+
+  /**
+   * Returns whether the elevator has hit the lower limit.
+   *
+   * @param x The current elevator state.
+   * @return Whether the elevator has hit the lower limit.
+   */
+  bool HasHitLowerLimit(const Eigen::Matrix<double, 2, 1>& x) const;
+
+  /**
+   * Returns whether the elevator has hit the upper limit.
+   *
+   * @param x The current elevator state.
+   * @return Whether the elevator has hit the uppwer limit.
+   */
+  bool HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const;
+
+  /**
+   * Returns the position of the elevator.
+   *
+   * @return The position of the elevator.
+   */
+  units::meter_t GetPosition() const;
+
+  /**
+   * Returns the velocity of the elevator.
+   *
+   * @return The velocity of the elevator.
+   */
+  units::meters_per_second_t GetVelocity() const;
+
+  /**
+   * Returns the elevator current draw.
+   *
+   * @return The elevator current draw.
+   */
+  units::ampere_t GetCurrentDraw() const override;
+
+ protected:
+  /**
+   * Updates the state estimate of the elevator.
+   *
+   * @param currentXhat The current state estimate.
+   * @param u           The system inputs (voltage).
+   * @param dt          The time difference between controller updates.
+   */
+  Eigen::Matrix<double, 2, 1> UpdateX(
+      const Eigen::Matrix<double, 2, 1>& currentXhat,
+      const Eigen::Matrix<double, 1, 1>& u, units::second_t dt) override;
+
+ private:
+  DCMotor m_motor;
+  units::meter_t m_drumRadius;
+  units::meter_t m_minHeight;
+  units::meter_t m_maxHeight;
+  double m_gearing;
+};
+}  // namespace frc::sim