用WPILib进行物理模拟

因为：ref：状态空间表示法`允许我们紧凑地表示：术语：`系统的动力学，我们可以利用它为模拟机器人上的物理系统提供后端。 这些模拟器的目的是在不修改现有的非仿真用户代码的情况下模拟机器人机构的运动。 这种模拟器的基本流程如下：

  • 在正常用户代码中：

    • PID或类似的控制算法从编码器（或其他传感器）读数中生成电压命令

    • 电机输出被设置

  • 在模拟周期代码中：

    • 模拟的：术语：状态 使用中：术语：`输入`更新的，通常是来自PID回路设置的电机的电压

    • 模拟编码器（或其他传感器）读数被设置为用户代码在下一个时间步骤中使用

WPILib的模拟类

WPILib提供以下物理模拟类：

  • LinearSystemSim，用于具有线性动力学的系统建模

  • FlywheelSim

  • DifferentialDrivetrainSim

  • ElevatorSim, which models gravity in the direction of elevator motion

  • SingleJointedArmSim, which models gravity proportional to the arm angle

  • BatterySim，仅根据汲取的电流估算电池电压骤降

所有模拟类（差速驱动器模拟器除外）都继承自:code:LinearSystemSim`类。默认情况下，动力学是线性系统动力学:math:mathbf{x}_{k+1} = mathbf{A}mathbf{x}_k + mathbf{B}mathbf{u}_k`。子类重写:code:UpdateX(x, u, dt) 方法，以提供自定义的非线性动力学，例如对重力建模。

备注

Swerve support for simulation is in the works, but we cannot provide an ETA. For updates on progress, please follow this pull request.

用户代码中的用法

WPILib中的示例项目<https://github.com/wpilibsuite/allwpilib/tree/main/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation>`__提供了以下内容。

In addition to standard objects such as motors and encoders, we instantiate our elevator simulator using known constants such as carriage mass and gearing reduction. We also instantiate an EncoderSim, which sets the distance and rate read by our Encoder.

在下面的例子中，我们模拟了一台电梯，给定了移动车厢的质量（以千克为单位）、驱动电梯的滚筒的半径（以米为单位）、电机和滚筒之间的齿轮减速作为输入输出（因此通常大于一）、电梯的最小和最大高度（以米为单位）以及要添加的一些随机噪声来估计我们的位置。

备注

电梯和手臂模拟器将防止模拟位置超过给定的最小或最大高度或角度。如果要模拟具有无限旋转或运动的机构，：code:`LinearSystemSim`可能是更好的选择。

47  // Simulation classes help us simulate what's going on, including gravity.
48  private final ElevatorSim m_elevatorSim =
49      new ElevatorSim(
50          m_elevatorGearbox,
51          Constants.kElevatorGearing,
52          Constants.kCarriageMass,
53          Constants.kElevatorDrumRadius,
54          Constants.kMinElevatorHeightMeters,
55          Constants.kMaxElevatorHeightMeters,
56          true,
57          VecBuilder.fill(0.01));
58  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);

下一步，代码：“遥循环”/代码：“遥循环”（java／C++）使用简单的PID控制回路来驱动我们的电梯到离地面30英寸的设置点。

31  @Override
32  public void teleopPeriodic() {
33    if (m_joystick.getTrigger()) {
34      // Here, we set the constant setpoint of 0.75 meters.
35      m_elevator.reachGoal(Constants.kSetpointMeters);
36    } else {
37      // Otherwise, we update the setpoint to 0.
38      m_elevator.reachGoal(0.0);
39    }
40  }

 98  public void reachGoal(double goal) {
 99    m_controller.setGoal(goal);
100
101    // With the setpoint value we run PID control like normal
102    double pidOutput = m_controller.calculate(m_encoder.getDistance());
103    double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
104    m_motor.setVoltage(pidOutput + feedforwardOutput);
105  }

其次，：code：simulationPeriodic/:code：`SimulationPeriodic`(JAVA/C++)利用施加在电机上的电压来更新电梯的模拟位置。我们使用的是：代码：“模拟周期”，因为它只为模拟机器人周期性地运行。这意味着我们的模拟代码不会在真正的机器人上运行。

备注

Classes inheriting from command-based’s Subsystem can override the inherited simulationPeriodic() method. Other classes will need their simulation update methods called from Robot’s simulationPeriodic.

最后，利用模拟电梯的位置设置模拟编码器的距离读数，利用电梯绘制的估计电流设置机器人的电池电压。

78  public void simulationPeriodic() {
79    // In this method, we update our simulation of what our elevator is doing
80    // First, we set our "inputs" (voltages)
81    m_elevatorSim.setInput(m_motorSim.getSpeed() * RobotController.getBatteryVoltage());
82
83    // Next, we update it. The standard loop time is 20ms.
84    m_elevatorSim.update(0.020);
85
86    // Finally, we set our simulated encoder's readings and simulated battery voltage
87    m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
88    // SimBattery estimates loaded battery voltages
89    RoboRioSim.setVInVoltage(
90        BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
91  }