-
Notifications
You must be signed in to change notification settings - Fork 34
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #22 from CrossTheRoadElec/SimAdditions
Sim additions
Showing
66 changed files
with
1,320 additions
and
139 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
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
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
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,24 @@ | ||
| { | ||
| "NetworkTables": { | ||
| "transitory": { | ||
| "SmartDashboard": { | ||
| "open": true | ||
| } | ||
| }, | ||
| "types": { | ||
| "/FMSInfo": "FMSInfo", | ||
| "/SmartDashboard/mech2d": "Mechanism2d" | ||
| }, | ||
| "windows": { | ||
| "/SmartDashboard/mech2d": { | ||
| "window": { | ||
| "visible": true | ||
| } | ||
| } | ||
| } | ||
| }, | ||
| "NetworkTables Settings": { | ||
| "mode": "Client (NT4)", | ||
| "serverTeam": "7762" | ||
| } | ||
| } |
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,43 @@ | ||
| package frc.robot; | ||
|
|
||
| import com.ctre.phoenix6.StatusSignal; | ||
|
|
||
| import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj.util.Color; | ||
| import edu.wpi.first.wpilibj.util.Color8Bit; | ||
|
|
||
| /** | ||
| * Class to keep all the mechanism-specific objects together and out of the main example | ||
| */ | ||
| public class Mechanisms { | ||
| private final double HEIGHT = 1; | ||
| private final double WIDTH = 1; | ||
| private final double ROOT_X = WIDTH / 2; | ||
| private final double ROOT_Y = HEIGHT / 2; | ||
|
|
||
| private Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); // Main mechanism object | ||
| private MechanismLigament2d wrist = mech.getRoot("base", ROOT_X, ROOT_Y) | ||
| .append(new MechanismLigament2d("Wrist", .25, 90, 6, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| @SuppressWarnings("unused") | ||
| private MechanismLigament2d leftArrow = wrist.append( | ||
| new MechanismLigament2d("LeftArrow", | ||
| 0.1, | ||
| 150, | ||
| 6, | ||
| new Color8Bit(Color.kAliceBlue))); | ||
| @SuppressWarnings("unused") | ||
| private MechanismLigament2d rightArrow = wrist.append( | ||
| new MechanismLigament2d("RightArrow", | ||
| 0.1, | ||
| -150, | ||
| 6, | ||
| new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| public void update(StatusSignal<Double> angle) { | ||
| SmartDashboard.putData("mech2d", mech); // Creates a mech2d window in GUI | ||
| wrist.setAngle(angle.getValue() * 360); // Converts 1 rotation to 360 degrees | ||
| } | ||
| } |
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.CANcoder; | ||
| 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, CANcoder can, double gearRatio, final double rotorInertia) { | ||
| if (falcon != null) { | ||
| TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, can, gearRatio, 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; | ||
| } | ||
| } | ||
| } |
67 changes: 67 additions & 0 deletions
67
java/CANcoder/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,67 @@ | ||
| package frc.robot.sim; | ||
|
|
||
| import com.ctre.phoenix6.hardware.CANcoder; | ||
| 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 static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation | ||
| private final TalonFX _falcon; | ||
| private final CANcoder _canCoder; | ||
| private final double _gearRatio; | ||
|
|
||
| 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 CANcoder canCoder, final double gearRatio, final double rotorInertia) { | ||
| this._falcon = falcon; | ||
| this._canCoder = canCoder; | ||
| this._gearRatio = gearRatio; | ||
| this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), gearRatio, .001); | ||
| } | ||
|
|
||
| /** | ||
| * 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 | ||
| final double position_rot = _motorSim.getAngularPositionRotations() * _gearRatio; | ||
| final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()) * _gearRatio; | ||
|
|
||
| _falcon.getSimState().setRawRotorPosition(position_rot); | ||
| _falcon.getSimState().setRotorVelocity(velocity_rps); | ||
|
|
||
| _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); | ||
|
|
||
| _canCoder.getSimState().setRawPosition(position_rot / _gearRatio); | ||
| _canCoder.getSimState().setVelocity(velocity_rps / _gearRatio); | ||
| } | ||
| } |
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
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
71 changes: 71 additions & 0 deletions
71
java/FusedCANcoder/src/main/java/frc/robot/Mechanisms.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,71 @@ | ||
| package frc.robot; | ||
|
|
||
| import com.ctre.phoenix6.StatusSignal; | ||
|
|
||
| import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj.util.Color; | ||
| import edu.wpi.first.wpilibj.util.Color8Bit; | ||
|
|
||
| /** | ||
| * Class to keep all the mechanism-specific objects together and out of the main example | ||
| */ | ||
| public class Mechanisms { | ||
| double HEIGHT = 1; //Controls the height of the mech2d SmartDashboard | ||
| double WIDTH = 1; //Controls the height of the mech2d SmartDashboard | ||
| double FX = 0; | ||
| double TALON = 0; | ||
| double CAN = 0; | ||
|
|
||
|
|
||
| Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); | ||
| /* rotor rotor Ligaments */ | ||
| MechanismLigament2d rotorArm = mech. | ||
| getRoot("rotorpivotPoint", 0.25, 0.75). | ||
| append(new MechanismLigament2d("rotorArm", .1, 0, 0, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| MechanismLigament2d rotorSide1 = rotorArm.append(new MechanismLigament2d("rotorSide1", 0.076535, 112.5, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d rotorSide2 = rotorSide1.append(new MechanismLigament2d("rotorSide2", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d rotorSide3 = rotorSide2.append(new MechanismLigament2d("rotorSide3", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d rotorSide4 = rotorSide3.append(new MechanismLigament2d("rotorSide4", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d rotorSide5 = rotorSide4.append(new MechanismLigament2d("rotorSide5", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d rotorSide6 = rotorSide5.append(new MechanismLigament2d("rotorSide6", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d rotorSide7 = rotorSide6.append(new MechanismLigament2d("rotorSide7", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d rotorSide8 = rotorSide7.append(new MechanismLigament2d("rotorSide8", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| /* CANcoder ligaments */ | ||
| MechanismLigament2d ccArm = mech. | ||
| getRoot("CANpivotPoint", 0.25, 0.25). | ||
| append(new MechanismLigament2d("ccArm", .1, 0, 0, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| MechanismLigament2d ccSide1 = ccArm.append(new MechanismLigament2d("ccSide1", 0.076535, 112.5, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d ccSide2 = ccSide1.append(new MechanismLigament2d("ccSide2", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d ccSide3 = ccSide2.append(new MechanismLigament2d("ccSide3", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d ccSide4 = ccSide3.append(new MechanismLigament2d("ccSide4", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d ccSide5 = ccSide4.append(new MechanismLigament2d("ccSide5", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d ccSide6 = ccSide5.append(new MechanismLigament2d("ccSide6", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d ccSide7 = ccSide6.append(new MechanismLigament2d("ccSide7", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d ccSide8 = ccSide7.append(new MechanismLigament2d("ccSide8", 0.076535, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| /* FX Mechanism Ligaments */ | ||
| MechanismLigament2d mechanismArm = mech. | ||
| getRoot("TALONpivotPoint", 0.75, 0.5). | ||
| append(new MechanismLigament2d("mechanismArm", .2, 0, 0, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| MechanismLigament2d mechanismSide1 = mechanismArm.append(new MechanismLigament2d("mechanismSide1", 0.15307, 112.5, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d mechanismSide2 = mechanismSide1.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d mechanismSide3 = mechanismSide2.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d mechanismSide4 = mechanismSide3.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d mechanismSide5 = mechanismSide4.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d mechanismSide6 = mechanismSide5.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d mechanismSide7 = mechanismSide6.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d mechanismSide8 = mechanismSide7.append(new MechanismLigament2d("mechanismSide2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
|
|
||
| public void update(StatusSignal<Double> fxRotorPosition, StatusSignal<Double> fxPosition, StatusSignal<Double> cancoderPosition) { | ||
|
|
||
| rotorArm.setAngle(fxRotorPosition.refresh().getValue() * 360); | ||
| mechanismArm.setAngle(fxPosition.refresh().getValue() * 360); | ||
| ccArm.setAngle(cancoderPosition.refresh().getValue() * 360); | ||
| SmartDashboard.putData("mech2d", mech); // Creates mech2d in SmartDashboard | ||
| } | ||
| } |
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
86 changes: 86 additions & 0 deletions
86
java/FusedCANcoder/src/main/java/frc/robot/sim/PhysicsSim.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,86 @@ | ||
| package frc.robot.sim; | ||
|
|
||
| import java.util.ArrayList; | ||
|
|
||
| import com.ctre.phoenix6.Utils; | ||
| import com.ctre.phoenix6.hardware.CANcoder; | ||
| 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, CANcoder can, double gearRatio, final double rotorInertia) { | ||
| if (falcon != null) { | ||
| TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, can, gearRatio, 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; | ||
| } | ||
| } | ||
| } |
69 changes: 69 additions & 0 deletions
69
java/FusedCANcoder/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,69 @@ | ||
| package frc.robot.sim; | ||
|
|
||
| import com.ctre.phoenix6.hardware.CANcoder; | ||
| 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 static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation | ||
| private final TalonFX _falcon; | ||
| private final CANcoder _canCoder; | ||
| private final double _gearRatio; | ||
|
|
||
| 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 CANcoder canCoder, final double gearRatio, final double rotorInertia) { | ||
| this._falcon = falcon; | ||
| this._canCoder = canCoder; | ||
| this._gearRatio = gearRatio; | ||
| this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), gearRatio, 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 | ||
| final double position_rot = _motorSim.getAngularPositionRotations() * _gearRatio; | ||
| final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()) * _gearRatio; | ||
|
|
||
| _falcon.getSimState().setRawRotorPosition(position_rot); | ||
| _falcon.getSimState().setRotorVelocity(velocity_rps); | ||
|
|
||
| _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); | ||
|
|
||
| _canCoder.getSimState().setRawPosition(position_rot / _gearRatio); | ||
| _canCoder.getSimState().setVelocity(velocity_rps / _gearRatio); | ||
|
|
||
| } | ||
| } |
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,54 @@ | ||
| package frc.robot; | ||
|
|
||
| import com.ctre.phoenix6.StatusSignal; | ||
|
|
||
| import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj.util.Color; | ||
| import edu.wpi.first.wpilibj.util.Color8Bit; | ||
|
|
||
| /** | ||
| * Class to keep all the mechanism-specific objects together and out of the main example | ||
| */ | ||
| public class Mechanisms { | ||
| double HEIGHT = 1; // Controls the height of the mech2d SmartDashboard | ||
| double WIDTH = 1; // Controls the height of the mech2d SmartDashboard | ||
|
|
||
| Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); | ||
| // Velocity | ||
| MechanismLigament2d VelocityMech = mech. | ||
| getRoot("velocityLineReferencePosition", 0.75, 0.5). | ||
| append(new MechanismLigament2d("velocityLine", 1,90, 6, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| MechanismLigament2d midline = mech. | ||
| getRoot("midline", 0.7, 0.5). | ||
| append(new MechanismLigament2d("midline", 0.1, 0, 3, new Color8Bit(Color.kCyan))); | ||
|
|
||
| //Position | ||
| MechanismLigament2d arm = mech. | ||
| getRoot("pivotPoint", 0.25, 0.5). | ||
| append(new MechanismLigament2d("arm", .2, 0, 0, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| MechanismLigament2d side1 = arm.append(new MechanismLigament2d("side1", 0.15307, 112.5, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side2 = side1.append(new MechanismLigament2d("side2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side3 = side2.append(new MechanismLigament2d("side3", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side4 = side3.append(new MechanismLigament2d("side4", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side5 = side4.append(new MechanismLigament2d("side5", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side6 = side5.append(new MechanismLigament2d("side6", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side7 = side6.append(new MechanismLigament2d("side7", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side8 = side7.append(new MechanismLigament2d("side8", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| /** | ||
| * Runs the mech2d widget in GUI. | ||
| * | ||
| * This utilizes GUI to simulate and display a TalonFX and exists to allow users to test and understand | ||
| * features of our products in simulation using our examples out of the box. Users may modify to have a | ||
| * display interface that they find more intuitive or visually appealing. | ||
| */ | ||
| public void update(StatusSignal<Double> position, StatusSignal<Double> velocity) { | ||
| VelocityMech.setLength(velocity.getValue()/120); // Divide by 120 to scale motion to fit in the window | ||
| arm.setAngle(position.getValue() * 360); | ||
| SmartDashboard.putData("mech2d", mech); // Creates mech2d in SmartDashboard | ||
| } | ||
| } |
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,34 @@ | ||
| package frc.robot; | ||
|
|
||
| import com.ctre.phoenix6.StatusSignal; | ||
|
|
||
| import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj.util.Color; | ||
| import edu.wpi.first.wpilibj.util.Color8Bit; | ||
|
|
||
| /** | ||
| * Class to keep all the mechanism-specific objects together and out of the main example | ||
| */ | ||
| public class Mechanisms { | ||
| private final double HEIGHT = 1; | ||
| private final double WIDTH = 1; | ||
| private final double ROOT_X = WIDTH / 2; | ||
| private final double ROOT_Y = HEIGHT / 2; | ||
|
|
||
| private Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); // Main mechanism object | ||
| private MechanismLigament2d wrist = mech. | ||
| getRoot("base", ROOT_X, ROOT_Y). | ||
| append(new MechanismLigament2d("Wrist", .25, 90, 6, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| @SuppressWarnings("unused") | ||
| private MechanismLigament2d leftArrow = wrist.append(new MechanismLigament2d("LeftArrow", 0.1, 150, 6, new Color8Bit(Color.kAliceBlue))); | ||
| @SuppressWarnings("unused") | ||
| private MechanismLigament2d rightArrow = wrist.append(new MechanismLigament2d("RightArrow", 0.1, -150, 6, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| public void update(StatusSignal<Double> position) { | ||
| wrist.setAngle(position.getValue()); | ||
| SmartDashboard.putData("mech2d", mech); | ||
| } | ||
| } |
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; | ||
| } | ||
| } | ||
| } |
66 changes: 66 additions & 0 deletions
66
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,66 @@ | ||
| 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 static 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.getFalcon500Foc(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 | ||
| final double position_rot = _motorSim.getAngularPositionRotations(); | ||
| final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); | ||
|
|
||
| _falcon.getSimState().setRawRotorPosition(position_rot); | ||
| _falcon.getSimState().setRotorVelocity(velocity_rps); | ||
|
|
||
| _falcon.getSimState().setSupplyVoltage(12 - _falcon.getSimState().getSupplyCurrent() * kMotorResistance); | ||
|
|
||
| _pigeon.getSimState().setRawYaw(position_rot); | ||
|
|
||
| } | ||
| } |
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,19 @@ | ||
| { | ||
| "NetworkTables": { | ||
| "types": { | ||
| "/FMSInfo": "FMSInfo", | ||
| "/SmartDashboard/mech2d": "Mechanism2d" | ||
| }, | ||
| "windows": { | ||
| "/SmartDashboard/mech2d": { | ||
| "window": { | ||
| "visible": true | ||
| } | ||
| } | ||
| } | ||
| }, | ||
| "NetworkTables Settings": { | ||
| "mode": "Client (NT4)", | ||
| "serverTeam": "7762" | ||
| } | ||
| } |
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
42 changes: 42 additions & 0 deletions
42
java/PositionClosedLoop/src/main/java/frc/robot/Mechanisms.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,42 @@ | ||
| package frc.robot; | ||
|
|
||
| import com.ctre.phoenix6.StatusSignal; | ||
|
|
||
| import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj.util.Color; | ||
| import edu.wpi.first.wpilibj.util.Color8Bit; | ||
|
|
||
| /** | ||
| * Class to keep all the mechanism-specific objects together and out of the main example | ||
| */ | ||
| public class Mechanisms { | ||
| private final double HEIGHT = .5; // Controls the height of the mech2d SmartDashboard | ||
| private final double WIDTH = 1; // Controls the height of the mech2d SmartDashboard | ||
|
|
||
| Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); | ||
| MechanismLigament2d distanceBar = mech. | ||
| getRoot("startPoint", 0.5, 0.4). | ||
| append(new MechanismLigament2d("distanceBar", 1, 0, 6, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| MechanismLigament2d reference = mech. | ||
| getRoot("baseLine", 0, .1). | ||
| append(new MechanismLigament2d("baseLine", 1, 0, 6, new Color8Bit(Color.kCyan))); | ||
|
|
||
| MechanismLigament2d joint = mech. | ||
| getRoot("neckLine", 0.5, .1). | ||
| append(new MechanismLigament2d("neckLine", 0.3, 90, 6, new Color8Bit(Color.kCyan))); | ||
|
|
||
| /** | ||
| * Runs the mech2d widget in GUI. | ||
| * | ||
| * This utilizes GUI to simulate and display a TalonFX and exists to allow users to test and understand | ||
| * features of our products in simulation using our examples out of the box. Users may modify to have a | ||
| * display interface that they find more intuitive or visually appealing. | ||
| */ | ||
| public void update(StatusSignal<Double> position) { | ||
| distanceBar.setLength(position.getValue()/30); // Divide by 30 to scale motion to fit in the window | ||
| SmartDashboard.putData("mech2d", mech); // Creates mech2d in SmartDashboard | ||
| } | ||
| } |
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
81 changes: 81 additions & 0 deletions
81
java/PositionClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.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,81 @@ | ||
| package frc.robot.sim; | ||
|
|
||
| import java.util.ArrayList; | ||
|
|
||
| import com.ctre.phoenix6.Utils; | ||
| 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 rotorInertia | ||
| * Rotational Inertia of the mechanism at the rotor | ||
| */ | ||
| public void addTalonFX(TalonFX falcon, final double rotorInertia) { | ||
| if (falcon != null) { | ||
| TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, 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; | ||
| } | ||
| } | ||
| } |
61 changes: 61 additions & 0 deletions
61
java/PositionClosedLoop/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,61 @@ | ||
| package frc.robot.sim; | ||
|
|
||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
| import com.ctre.phoenix6.sim.TalonFXSimState; | ||
|
|
||
| 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 static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation | ||
|
|
||
| private final DCMotorSim _motorSim; | ||
| private final TalonFXSimState _falconSim; | ||
|
|
||
| /** | ||
| * 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 double rotorInertia) { | ||
| this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 1.0, rotorInertia); | ||
| this._falconSim = falcon.getSimState(); | ||
| } | ||
|
|
||
| /** | ||
| * 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(_falconSim.getMotorVoltage()); | ||
|
|
||
| _motorSim.update(getPeriod()); | ||
|
|
||
| /// SET SIM PHYSICS INPUTS | ||
| final double position_rot = _motorSim.getAngularPositionRotations(); | ||
| final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); | ||
|
|
||
| _falconSim.setRawRotorPosition(position_rot); | ||
| _falconSim.setRotorVelocity(velocity_rps); | ||
|
|
||
| _falconSim.setSupplyVoltage(12 - _falconSim.getSupplyCurrent() * kMotorResistance); | ||
| } | ||
| } |
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,19 @@ | ||
| { | ||
| "NetworkTables": { | ||
| "types": { | ||
| "/FMSInfo": "FMSInfo", | ||
| "/SmartDashboard/mech2d": "Mechanism2d" | ||
| }, | ||
| "windows": { | ||
| "/SmartDashboard/mech2d": { | ||
| "window": { | ||
| "visible": true | ||
| } | ||
| } | ||
| } | ||
| }, | ||
| "NetworkTables Settings": { | ||
| "mode": "Client (NT4)", | ||
| "serverTeam": "7762" | ||
| } | ||
| } |
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
54 changes: 54 additions & 0 deletions
54
java/VelocityClosedLoop/src/main/java/frc/robot/Mechanisms.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,54 @@ | ||
| package frc.robot; | ||
|
|
||
| import com.ctre.phoenix6.StatusSignal; | ||
|
|
||
| import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import edu.wpi.first.wpilibj.util.Color; | ||
| import edu.wpi.first.wpilibj.util.Color8Bit; | ||
|
|
||
| /** | ||
| * Class to keep all the mechanism-specific objects together and out of the main example | ||
| */ | ||
| public class Mechanisms { | ||
| double HEIGHT = 1; // Controls the height of the mech2d SmartDashboard | ||
| double WIDTH = 1; // Controls the height of the mech2d SmartDashboard | ||
|
|
||
| Mechanism2d mech = new Mechanism2d(WIDTH, HEIGHT); | ||
| // Velocity | ||
| MechanismLigament2d VelocityMech = mech. | ||
| getRoot("velocityLineReferencePosition", 0.75, 0.5). | ||
| append(new MechanismLigament2d("velocityLine", 1,90, 6, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| MechanismLigament2d midline = mech. | ||
| getRoot("midline", 0.7, 0.5). | ||
| append(new MechanismLigament2d("midline", 0.1, 0, 3, new Color8Bit(Color.kCyan))); | ||
|
|
||
| //Position | ||
| MechanismLigament2d arm = mech. | ||
| getRoot("pivotPoint", 0.25, 0.5). | ||
| append(new MechanismLigament2d("arm", .2, 0, 0, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| MechanismLigament2d side1 = arm.append(new MechanismLigament2d("side1", 0.15307, 112.5, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side2 = side1.append(new MechanismLigament2d("side2", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side3 = side2.append(new MechanismLigament2d("side3", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side4 = side3.append(new MechanismLigament2d("side4", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side5 = side4.append(new MechanismLigament2d("side5", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side6 = side5.append(new MechanismLigament2d("side6", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side7 = side6.append(new MechanismLigament2d("side7", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
| MechanismLigament2d side8 = side7.append(new MechanismLigament2d("side8", 0.15307, 45, 6, new Color8Bit(Color.kAliceBlue))); | ||
|
|
||
| /** | ||
| * Runs the mech2d widget in GUI. | ||
| * | ||
| * This utilizes GUI to simulate and display a TalonFX and exists to allow users to test and understand | ||
| * features of our products in simulation using our examples out of the box. Users may modify to have a | ||
| * display interface that they find more intuitive or visually appealing. | ||
| */ | ||
| public void update(StatusSignal<Double> position, StatusSignal<Double> velocity) { | ||
| VelocityMech.setLength(velocity.getValue()/120); // Divide by 120 to scale motion to fit in the window | ||
| arm.setAngle(position.getValue() * 360); | ||
| SmartDashboard.putData("mech2d", mech); // Creates mech2d in SmartDashboard | ||
| } | ||
| } |
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
81 changes: 81 additions & 0 deletions
81
java/VelocityClosedLoop/src/main/java/frc/robot/sim/PhysicsSim.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,81 @@ | ||
| package frc.robot.sim; | ||
|
|
||
| import java.util.ArrayList; | ||
|
|
||
| import com.ctre.phoenix6.Utils; | ||
| 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 rotorInertia | ||
| * Rotational Inertia of the mechanism at the rotor | ||
| */ | ||
| public void addTalonFX(TalonFX falcon, final double rotorInertia) { | ||
| if (falcon != null) { | ||
| TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, 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; | ||
| } | ||
| } | ||
| } |
60 changes: 60 additions & 0 deletions
60
java/VelocityClosedLoop/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,60 @@ | ||
| package frc.robot.sim; | ||
|
|
||
| import com.ctre.phoenix6.hardware.TalonFX; | ||
| import com.ctre.phoenix6.sim.TalonFXSimState; | ||
|
|
||
| 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 static final double kMotorResistance = 0.002; // Assume 2mOhm resistance for voltage drop calculation | ||
| private final TalonFXSimState _falconSim; | ||
| 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 double rotorInertia) { | ||
| this._motorSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), 1.0, rotorInertia); | ||
| this._falconSim = falcon.getSimState(); | ||
| } | ||
|
|
||
| /** | ||
| * 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(_falconSim.getMotorVoltage()); | ||
|
|
||
| _motorSim.update(getPeriod()); | ||
|
|
||
| /// SET SIM PHYSICS INPUTS | ||
| final double position_rot = _motorSim.getAngularPositionRotations(); | ||
| final double velocity_rps = Units.radiansToRotations(_motorSim.getAngularVelocityRadPerSec()); | ||
|
|
||
| _falconSim.setRawRotorPosition(position_rot); | ||
| _falconSim.setRotorVelocity(velocity_rps); | ||
|
|
||
| _falconSim.setSupplyVoltage(12 - _falconSim.getSupplyCurrent() * kMotorResistance); | ||
| } | ||
| } |
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