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.

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.

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          0,
58          VecBuilder.fill(0.01));
59  private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);

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

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  }

 99  public void reachGoal(double goal) {
100    m_controller.setGoal(goal);
101
102    // With the setpoint value we run PID control like normal
103    double pidOutput = m_controller.calculate(m_encoder.getDistance());
104    double feedforwardOutput = m_feedforward.calculate(m_controller.getSetpoint().velocity);
105    m_motor.setVoltage(pidOutput + feedforwardOutput);
106  }

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}

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.

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

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}