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

La prise en charge de la simulation par Swerve est en cours d’élaboration, mais nous ne pouvons pas fournir d’ETA. Pour des mises à jour sur les progrès, veuillez suivre ce pull request.

Utilisation dans le code utilisateur

Ce qui suit est disponible à partir du projet exemple de elevatorsimulation de WPILib.

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.

Dans l’exemple suivant, nous simulons un élévateur étant donné la masse du chariot mobile (en kilogrammes), le rayon du tambour entraînant l’élévateur (en mètres), la réduction de l’engrenage entre le moteur et le tambour en sortie sur entrée (donc généralement supérieure à one), la hauteur minimale et maximale de l’élévateur (en mètres) et du bruit aléatoire à ajouter à notre estimation de position.

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.

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);

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.

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  }

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.

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  }