-
Notifications
You must be signed in to change notification settings - Fork 25
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
1 parent
9f8adee
commit 5225e1f
Showing
11 changed files
with
201 additions
and
86 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,86 @@ | ||
| package frc.robot.sim; | ||
|
|
||
| import java.util.ArrayList; | ||
|
|
||
| import com.ctre.phoenix6.Utils; | ||
| import com.ctre.phoenix6.hardware.Pigeon2; | ||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
|
|
||
| /** | ||
| * Manages physics simulation for CTRE products. | ||
| */ | ||
| public class PhysicsSim { | ||
| private static final PhysicsSim sim = new PhysicsSim(); | ||
|
|
||
| /** | ||
| * Gets the robot simulator instance. | ||
| */ | ||
| public static PhysicsSim getInstance() { | ||
| return sim; | ||
| } | ||
|
|
||
| /** | ||
| * Adds a TalonFX controller to the simulator. | ||
| * | ||
| * @param falcon | ||
| * The TalonFX device | ||
| * @param can | ||
| * The CANcoder device | ||
| * @param gearRatio | ||
| * The gear reduction of the TalonFX | ||
| * @param rotorInertia | ||
| * Rotational Inertia of the mechanism at the rotor | ||
| */ | ||
| public void addTalonFX(TalonFX falcon, Pigeon2 pigeon, final double rotorInertia) { | ||
| if (falcon != null) { | ||
| TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, pigeon, rotorInertia); | ||
| _simProfiles.add(simFalcon); | ||
| } | ||
| } | ||
|
|
||
| /** | ||
| * Runs the simulator: | ||
| * - enable the robot | ||
| * - simulate sensors | ||
| */ | ||
| public void run() { | ||
| // Simulate devices | ||
| for (SimProfile simProfile : _simProfiles) { | ||
| simProfile.run(); | ||
| } | ||
| } | ||
|
|
||
| private final ArrayList<SimProfile> _simProfiles = new ArrayList<SimProfile>(); | ||
|
|
||
| /** | ||
| * Holds information about a simulated device. | ||
| */ | ||
| static class SimProfile { | ||
| private double _lastTime; | ||
| private boolean _running = false; | ||
|
|
||
| /** | ||
| * Runs the simulation profile. | ||
| * Implemented by device-specific profiles. | ||
| */ | ||
| public void run() { | ||
| } | ||
|
|
||
| /** | ||
| * Returns the time since last call, in seconds. | ||
| */ | ||
| protected double getPeriod() { | ||
| // set the start time if not yet running | ||
| if (!_running) { | ||
| _lastTime = Utils.getCurrentTimeSeconds(); | ||
| _running = true; | ||
| } | ||
|
|
||
| double now = Utils.getCurrentTimeSeconds(); | ||
| final double period = now - _lastTime; | ||
| _lastTime = now; | ||
|
|
||
| return period; | ||
| } | ||
| } | ||
| } |
65 changes: 65 additions & 0 deletions
65
java/Pigeon2/src/main/java/frc/robot/sim/TalonFXSimProfile.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,65 @@ | ||
| package frc.robot.sim; | ||
|
|
||
| import com.ctre.phoenix6.hardware.Pigeon2; | ||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
|
|
||
| import edu.wpi.first.math.system.plant.DCMotor; | ||
| import edu.wpi.first.math.util.Units; | ||
| import edu.wpi.first.wpilibj.simulation.DCMotorSim; | ||
| import frc.robot.sim.PhysicsSim.SimProfile; | ||
|
|
||
| /** | ||
| * Holds information about a simulated TalonFX. | ||
| */ | ||
| class TalonFXSimProfile extends SimProfile { | ||
| private final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation | ||
| private final TalonFX _falcon; | ||
| private final Pigeon2 _pigeon; | ||
|
|
||
| private final DCMotorSim _motorSim; | ||
|
|
||
| /** | ||
| * Creates a new simulation profile for a TalonFX device. | ||
| * | ||
| * @param falcon | ||
| * The TalonFX device | ||
| * @param accelToFullTime | ||
| * The time the motor takes to accelerate from 0 to full, | ||
| * in seconds | ||
| * @param fullVel | ||
| * The maximum motor velocity, in rotations per second | ||
| * @param sensorPhase | ||
| * The phase of the TalonFX sensors | ||
| */ | ||
| public TalonFXSimProfile(final TalonFX falcon, final Pigeon2 pigeon, final double rotorInertia) { | ||
| this._falcon = falcon; | ||
| this._pigeon = pigeon; | ||
| this._motorSim = new DCMotorSim(DCMotor.getFalcon500(1), 1.0, rotorInertia); | ||
| } | ||
|
|
||
| /** | ||
| * Runs the simulation profile. | ||
| * | ||
| * This uses very rudimentary physics simulation and exists to allow users to | ||
| * test features of our products in simulation using our examples out of the | ||
| * box. Users may modify this to utilize more accurate physics simulation. | ||
| */ | ||
| public void run() { | ||
| /// DEVICE SPEED SIMULATION | ||
|
|
||
| _motorSim.setInputVoltage(_falcon.getSimState().getMotorVoltage()); | ||
|
|
||
| _motorSim.update(getPeriod()); | ||
|
|
||
| /// SET SIM PHYSICS INPUTS | ||
| double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); | ||
|
|
||
| _falcon.getSimState().setRawRotorPosition(_motorSim.getAngularPositionRotations()); | ||
| _falcon.getSimState().setRotorVelocity(velocity_rps); | ||
|
|
||
| _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); | ||
|
|
||
| _pigeon.getSimState().setRawYaw(_motorSim.getAngularPositionRotations()); | ||
|
|
||
| } | ||
| } |
Oops, something went wrong.