Simulación física con WPILib

Debido a que la :ref: notación de espacio de estado <docs/software/advanced-controls/state-space/state-space-intro:What is State-Space Notation?> nos permite representar de manera compacta el dinámica de sistemas, podemos aprovecharlo para proporcionar un backend para simular sistemas físicos en robots. El objetivo de estos simuladores es simular el movimiento de los mecanismos del robot sin modificar el código de usuario existente que no sea de simulación. El flujo básico de dichos simuladores es el siguiente:

  • En código de usuario normal:

    • PID o algoritmos de control similares generan comandos de voltaje a partir de lecturas del encoder (u otro sensor)

    • Los valores de salida del motor están configurados

  • En el código periódico de simulación:

    • El estado de la simulación se actualiza usando entradas, generalmente voltajes de motores configurados desde un bucle PID

    • Las lecturas del encoder (u otro sensor) se configuran para que el código de usuario las utilice en el siguiente paso de tiempo

Clases de simulación de WPILib

Las siguientes clases de simulación física están disponibles en WPILib:

  • LinearSystemSim, para simular sistemas con dinámica lineal

  • FlywheelSim

  • DifferentialDrivetrainSim

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

  • SingleJointedArmSim, which models gravity proportional to the arm angle

  • BatterySim, que simplemente estima la caída de voltaje de la batería en función de las corrientes dibujadas

Todas las clases de simulación (con la excepción del simulador de diferential drive) heredan de la clase LinearSystemSim. Por defecto, la dinámica es la dinámica del sistema lineal \(\mathbf{x}_{k+1} = \mathbf{A}\mathbf{x}_k + \mathbf{B}\mathbf{u}_k\). Las subclases anulan el método UpdateX(x, u, dt) para proporcionar dinámicas personalizadas y no lineales, como simular la gravedad.

Nota

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

Uso en el código de usuario

Lo siguiente está disponible en el WPILib elevatorsimulation proyecto de ejemplo.

Además de los objetos estándar como motores y encoders, instanciamos nuestro simulador de elevador utilizando constantes conocidas como la masa del carro y la reducción de engranajes. También creamos una instancia de un EncoderSim, que establece la distancia y velocidad leídas por nuestro Encoder.

En el siguiente ejemplo, simulamos un elevador dada la masa del carro en movimiento (en kilogramos), el radio del tambor que impulsa el elevador (en metros), la reducción de engranajes entre el motor y el tambor como salida sobre la entrada (por lo que generalmente es mayor que uno), la altura mínima y máxima del ascensor (en metros), y algún ruido aleatorio para agregar a nuestra estimación de posición.

Nota

Los simuladores de elevador y brazo evitarán que la posición simulada supere las alturas o ángulos mínimos o máximos dados. Si desea simular un mecanismo con rotación o movimiento infinito, LinearSystemSim puede ser una mejor opción.

53  // Simulation classes help us simulate what's going on, including gravity.
54  private final ElevatorSim m_elevatorSim =
55      new ElevatorSim(
56          m_elevatorGearbox,
57          kElevatorGearing,
58          kCarriageMass,
59          kElevatorDrumRadius,
60          kMinElevatorHeight,
61          kMaxElevatorHeight,
62          VecBuilder.fill(0.01));
63  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);

A continuación, teleopPeriodic/TeleopPeriodic (Java/C++) utiliza un circuito de control PID simple para conducir nuestro elevador a un punto de ajuste de 30 pulgadas sobre el suelo.

100  @Override
101  public void teleopPeriodic() {
102    if (m_joystick.getTrigger()) {
103      // Here, we run PID control like normal, with a constant setpoint of 30in.
104      double pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.inchesToMeters(30));
105      m_motor.setVoltage(pidOutput);
106    } else {
107      // Otherwise, we disable the motor.
108      m_motor.set(0.0);
109    }
110  }

A continuación, simulationPeriodic/SimulationPeriodic (Java/C++) utiliza el voltaje aplicado al motor para actualizar la posición simulada del elevador. Usamos :código:`SimulationPeriodic` porque se ejecuta periódicamente solo para robots simulados. Esto significa que nuestro código de simulación no se ejecutará en un robot real.

Finalmente, la lectura de distancia del encoder se establece usando la posición del elevador simulado, y el voltaje de la batería del robot se establece usando la corriente estimada consumida por el elevador.

81  @Override
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_motor.get() * 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
96    // Update elevator visualization with simulated position
97    m_elevatorMech2d.setLength(Units.metersToInches(m_elevatorSim.getPositionMeters()));
98  }