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.

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.

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.

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

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.

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  }

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.

Nota

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.

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.

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  }