Simulation physique avec WPILib

Parce que la notation de l’espace d’états nous permet de représenter de manière compacte la dynamique d’un système, nous pouvons en tirer parti pour fournir une façon de simuler des systèmes physiques sur des robots. Le but de ces simulateurs est de simuler le mouvement des mécanismes du robot sans modifier le code utilisateur existant. L’algorithme de base de ces simulateurs est le suivant:

  • En code utilisateur normal:

    • Le PID ou des algorithmes de contrôle similaires génèrent des commandes de tension à partir des lectures du codeur (ou d’un autre capteur)

    • Les sorties du moteur sont réglées

  • Dans le code périodique de simulation:

    • La simulation état est mise à jour en utilisant les Entrées, généralement des tensions de moteurs réglés à partir d’une boucle PID

    • Les lectures de l’encodeur simulé (ou d’un autre capteur) sont définies pour le code utilisateur à utiliser dans le prochain incrément de temps

Classes de simulation de WPILib

Les classes de simulation physique suivantes sont disponibles dans WPILib:

  • LinearSystemSim, pour la modélisation de systèmes avec dynamique linéaire

  • Volant d’inertie

  • DifférentielDrivetrainSim

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

  • SingleJointedArmSim, which models gravity proportional to the arm angle

  • BatterySim, qui estime simplement l’affaissement de la tension de la batterie en fonction des courants tirés

Toutes les classes de simulation (à l’exception du simulateur d’entraînement différentiel) héritent de la classe LinearSystemSim. Par défaut, la dynamique est la dynamique du système linéaire \(\mathbf{x}_{k+1} = \mathbf{A}\mathbf{x}_k + \mathbf{B}\mathbf{u}_k\). Les sous-classes remplacent la méthode UpdateX(x, u, dt) pour fournir une dynamique non linéaire personnalisée, comme la modélisation de la gravité.

Note

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

Utilisation dans le code utilisateur

The following is available from the WPILib elevatorsimulation example project.

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.

In the following example, we simulate an elevator given the mass of the moving carriage (in kilograms), the radius of the drum driving the elevator (in meters), the gearing reduction between motor and drum as output over input (so usually greater than one), the minimum and maximum height of the elevator (in meters), the starting height of the elevator, and some random noise to add to our position estimate.

Note

Les simulateurs d’élévateur et de bras empêcheront la position simulée de dépasser des hauteurs ou des angles minimum ou maximum donnés. Si vous souhaitez simuler un mécanisme avec une rotation ou un mouvement infini, LinearSystemSim peut être une meilleure option.

49  // Simulation classes help us simulate what's going on, including gravity.
50  private final ElevatorSim m_elevatorSim =
51      new ElevatorSim(
52          m_elevatorGearbox,
53          Constants.kElevatorGearing,
54          Constants.kCarriageMass,
55          Constants.kElevatorDrumRadius,
56          Constants.kMinElevatorHeightMeters,
57          Constants.kMaxElevatorHeightMeters,
58          true,
59          0,
60          0.01,
61          0.0);

51  // Simulation classes help us simulate what's going on, including gravity.
52  frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
53                                      Constants::kElevatorGearing,
54                                      Constants::kCarriageMass,
55                                      Constants::kElevatorDrumRadius,
56                                      Constants::kMinElevatorHeight,
57                                      Constants::kMaxElevatorHeight,
58                                      true,
59                                      0_m,
60                                      {0.01}};
61  frc::sim::EncoderSim m_encoderSim{m_encoder};

Ensuite, teleopPeriodic/TeleopPeriodic (Java/C++) utilise une simple boucle de contrôle PID pour conduire notre elevateur à un point de consigne à 30 pouces (76 cm) du sol.

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

102  public void reachGoal(double goal) {
103    m_controller.setGoal(goal);
104
105    // With the setpoint value we run PID control like normal
106    double pidOutput = m_controller.calculate(m_encoder.getDistance());
107    double feedforwardOutput =
108        m_feedforward.calculate(MetersPerSecond.of(m_controller.getSetpoint().velocity)).in(Volts);
109    m_motor.setVoltage(pidOutput + feedforwardOutput);

20void Robot::TeleopPeriodic() {
21  if (m_joystick.GetTrigger()) {
22    // Here, we set the constant setpoint of 0.75 meters.
23    m_elevator.ReachGoal(Constants::kSetpoint);
24  } else {
25    // Otherwise, we update the setpoint to 0.
26    m_elevator.ReachGoal(0.0_m);
27  }
28}

42void Elevator::ReachGoal(units::meter_t goal) {
43  m_controller.SetGoal(goal);
44  // With the setpoint value we run PID control like normal
45  double pidOutput =
46      m_controller.Calculate(units::meter_t{m_encoder.GetDistance()});
47  units::volt_t feedforwardOutput =
48      m_feedforward.Calculate(m_controller.GetSetpoint().velocity);
49  m_motor.SetVoltage(units::volt_t{pidOutput} + feedforwardOutput);
50}

Puis, simulationPeriodic/SimulationPeriodic (Java/C++) utilise la tension appliquée au moteur pour mettre à jour la position simulée de l’élévateur. Nous utilisons SimulationPeriodic car il s’exécute seulement que pour les robots simulés. Cela signifie que notre code de simulation ne sera pas exécuté sur un vrai robot.

Note

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.

Enfin, la lecture de la distance du codeur simulé est réglée en utilisant la position simulée de l’élévateur, et la tension de la batterie du robot est réglée en utilisant le courant estimé tiré par l’ascenseur.

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

20void Elevator::SimulationPeriodic() {
21  // In this method, we update our simulation of what our elevator is doing
22  // First, we set our "inputs" (voltages)
23  m_elevatorSim.SetInput(frc::Vectord<1>{
24      m_motorSim.GetSpeed() * frc::RobotController::GetInputVoltage()});
25
26  // Next, we update it. The standard loop time is 20ms.
27  m_elevatorSim.Update(20_ms);
28
29  // Finally, we set our simulated encoder's readings and simulated battery
30  // voltage
31  m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
32  // SimBattery estimates loaded battery voltages
33  frc::sim::RoboRioSim::SetVInVoltage(
34      frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
35}