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:
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, which models gravity in the direction of elevator motion
SingleJointedArmSim, which models gravity proportional to the arm angle
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é.
Note
La prise en charge de la simulation par Swerve est en cours d’élaboration, mais nous ne pouvons pas fournir d’ETA. Pour des mises à jour sur les progrès, veuillez suivre ce pull request.
Utilisation dans le code utilisateur
Ce qui suit est disponible à partir du projet exemple de elevatorsimulation de WPILib.
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.
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.
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};
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.
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}
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.
Note
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.
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.
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}