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:
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);
60 // Simulation classes help us simulate what's going on, including gravity.
61 frc::sim::ElevatorSim m_elevatorSim{m_elevatorGearbox,
62 kElevatorGearing,
63 kCarriageMass,
64 kElevatorDrumRadius,
65 kMinElevatorHeight,
66 kMaxElevatorHeight,
67 {0.01}};
68 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.
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 }
100 frc::sim::RoboRioSim::SetVInVoltage(
101 frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
102
103 // Update the Elevator length based on the simulated elevator height
104 m_elevatorMech2d->SetLength(
105 units::inch_t(m_elevatorSim.GetPosition()).value());
106 }
107
108 void TeleopPeriodic() override {
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 }
87 void SimulationPeriodic() override {
88 // In this method, we update our simulation of what our elevator is doing
89 // First, we set our "inputs" (voltages)
90 m_elevatorSim.SetInput(Eigen::Vector<double, 1>{
91 m_motor.Get() * frc::RobotController::GetInputVoltage()});
92
93 // Next, we update it. The standard loop time is 20ms.
94 m_elevatorSim.Update(20_ms);
95
96 // Finally, we set our simulated encoder's readings and simulated battery
97 // voltage
98 m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
99 // SimBattery estimates loaded battery voltages
100 frc::sim::RoboRioSim::SetVInVoltage(
101 frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
102
103 // Update the Elevator length based on the simulated elevator height
104 m_elevatorMech2d->SetLength(
105 units::inch_t(m_elevatorSim.GetPosition()).value());
106 }