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, qui modélise la gravité

  • SingleJointedArmSim, qui modélise la gravité

  • 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é.

Utilisation dans le code utilisateur

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

En plus des objets standard tels que les moteurs et les encodeurs, nous instancions notre simulateur de mécanisme élévateur en utilisant des constantes connues telles que la masse du chariot et la réduction de la vitesse. Nous instancions également un EncoderSim, qui définit la distance et la vélocité lues par notre 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.

36
37
38
39
40
41
42
43
  private final ElevatorSim m_elevatorSim = new ElevatorSim(m_elevatorGearbox,
      kElevatorGearing,
      kCarriageMass,
      kElevatorDrumRadius,
      kMinElevatorHeight,
      kMaxElevatorHeight,
      VecBuilder.fill(0.01));
  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.

36
37
38
39
40
41
42
43
44
45
46
  @Override
  public void teleopPeriodic() {
    if (m_joystick.getTrigger()) {
      // Here, we run PID control like normal, with a constant setpoint of 30in.
      double pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.inchesToMeters(30));
      m_motor.setVoltage(pidOutput);
    } else {
      // Otherwise, we disable the motor.
      m_motor.set(0.0);
    }
  }

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.

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.

68
69
70
71
72
73
74
75
76
77
78
79
80
81
  @Override
  public void simulationPeriodic() {
    // In this method, we update our simulation of what our elevator is doing
    // First, we set our "inputs" (voltages)
    m_elevatorSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());

    // Next, we update it. The standard loop time is 20ms.
    m_elevatorSim.update(0.020);

    // Finally, we set our simulated encoder's readings and simulated battery voltage
    m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
    // SimBattery estimates loaded battery voltages
    RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
  }