();
+
+ /**
+ * Gets the robot simulator instance.
+ */
+ public static PhysicsSim getInstance()
+ {
+ return sim;
+ }
+
+ /* scales a random domain of [0, 2pi] to [min, max] while prioritizing the peaks */
+ static double random(double min, double max)
+ {
+ return (max - min) / 2 * Math.sin(Math.IEEEremainder(Math.random(), 2 * 3.14159)) + (max + min) / 2;
+ }
+
+ static double random(double max)
+ {
+ return random(0, max);
+ }
+
+ /**
+ * Adds a TalonSRX controller to the simulator.
+ *
+ * @param talon The TalonSRX device
+ * @param accelToFullTime The time the motor takes to accelerate from 0 to full, in seconds
+ * @param fullVel The maximum motor velocity, in ticks per 100ms
+ */
+ public void addTalonSRX(TalonSRX talon, final double accelToFullTime, final double fullVel)
+ {
+ addTalonSRX(talon, accelToFullTime, fullVel, false);
+ }
+
+ /**
+ * Adds a TalonSRX controller to the simulator.
+ *
+ * @param talon The TalonSRX device
+ * @param accelToFullTime The time the motor takes to accelerate from 0 to full, in seconds
+ * @param fullVel The maximum motor velocity, in ticks per 100ms
+ * @param sensorPhase The phase of the TalonSRX sensors
+ */
+ public void addTalonSRX(TalonSRX talon, final double accelToFullTime, final double fullVel, final boolean sensorPhase)
+ {
+ if (talon != null)
+ {
+ TalonSRXSimProfile simTalon = new TalonSRXSimProfile(talon, accelToFullTime, fullVel, sensorPhase);
+ _simProfiles.add(simTalon);
+ }
+ }
+
+ /**
+ * Adds a TalonFX controller to the simulator.
+ *
+ * @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 ticks per 100ms
+ */
+ public void addTalonFX(TalonFX falcon, final double accelToFullTime, final double fullVel)
+ {
+ addTalonFX(falcon, accelToFullTime, fullVel, false);
+ }
+
+ /**
+ * Adds a TalonFX controller to the simulator.
+ *
+ * @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 ticks per 100ms
+ * @param sensorPhase The phase of the TalonFX sensors
+ */
+ public void addTalonFX(TalonFX falcon, final double accelToFullTime, final double fullVel, final boolean sensorPhase)
+ {
+ if (falcon != null)
+ {
+ TalonFXSimProfile simFalcon = new TalonFXSimProfile(falcon, accelToFullTime, fullVel, sensorPhase);
+ _simProfiles.add(simFalcon);
+ }
+ }
+
+ /**
+ * Adds a VictorSPX controller to the simulator.
+ *
+ * @param victor The VictorSPX device
+ */
+ public void addVictorSPX(VictorSPX victor)
+ {
+ if (victor != null)
+ {
+ VictorSPXSimProfile simVictor = new VictorSPXSimProfile(victor);
+ _simProfiles.add(simVictor);
+ }
+ }
+
+ /**
+ * Runs the simulator: - enable the robot - simulate sensors
+ */
+ public void run()
+ {
+ // Simulate devices
+ for (SimProfile simProfile : _simProfiles)
+ {
+ simProfile.run();
+ }
+ }
+
+ /**
+ * Holds information about a simulated device.
+ */
+ static class SimProfile
+ {
+
+ private long _lastTime;
+ private boolean _running = false;
+
+ /**
+ * Runs the simulation profile. Implemented by device-specific profiles.
+ */
+ public void run()
+ {
+ }
+
+ /**
+ * Returns the time since last call, in milliseconds.
+ */
+ protected double getPeriod()
+ {
+ // set the start time if not yet running
+ if (!_running)
+ {
+ _lastTime = System.nanoTime();
+ _running = true;
+ }
+
+ long now = System.nanoTime();
+ final double period = (now - _lastTime) / 1000000.;
+ _lastTime = now;
+
+ return period;
+ }
+ }
+}
\ No newline at end of file
diff --git a/swervelib/simulation/ctre/TalonFXSimProfile.java b/swervelib/simulation/ctre/TalonFXSimProfile.java
new file mode 100644
index 0000000..46f6b02
--- /dev/null
+++ b/swervelib/simulation/ctre/TalonFXSimProfile.java
@@ -0,0 +1,88 @@
+package swervelib.simulation.ctre;
+
+import static swervelib.simulation.ctre.PhysicsSim.random;
+
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import swervelib.simulation.ctre.PhysicsSim.SimProfile;
+
+/**
+ * Holds information about a simulated TalonFX.
+ */
+class TalonFXSimProfile extends SimProfile
+{
+
+ private final TalonFX _falcon;
+ private final double _accelToFullTime;
+ private final double _fullVel;
+ private final boolean _sensorPhase;
+
+ /** The current position */
+ // private double _pos = 0;
+ /**
+ * The current velocity
+ */
+ private double _vel = 0;
+
+ /**
+ * 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 ticks per 100ms
+ * @param sensorPhase The phase of the TalonFX sensors
+ */
+ public TalonFXSimProfile(final TalonFX falcon, final double accelToFullTime, final double fullVel,
+ final boolean sensorPhase)
+ {
+ this._falcon = falcon;
+ this._accelToFullTime = accelToFullTime;
+ this._fullVel = fullVel;
+ this._sensorPhase = sensorPhase;
+ }
+
+ /**
+ * 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()
+ {
+ final double period = getPeriod();
+ final double accelAmount = _fullVel / _accelToFullTime * period / 1000;
+
+ /// DEVICE SPEED SIMULATION
+
+ double outPerc = _falcon.getSimCollection().getMotorOutputLeadVoltage() / 12;
+ if (_sensorPhase)
+ {
+ outPerc *= -1;
+ }
+ // Calculate theoretical velocity with some randomness
+ double theoreticalVel = outPerc * _fullVel * random(0.95, 1);
+ // Simulate motor load
+ if (theoreticalVel > _vel + accelAmount)
+ {
+ _vel += accelAmount;
+ } else if (theoreticalVel < _vel - accelAmount)
+ {
+ _vel -= accelAmount;
+ } else
+ {
+ _vel += 0.9 * (theoreticalVel - _vel);
+ }
+ // _pos += _vel * period / 100;
+
+ /// SET SIM PHYSICS INPUTS
+
+ _falcon.getSimCollection().addIntegratedSensorPosition((int) (_vel * period / 100));
+ _falcon.getSimCollection().setIntegratedSensorVelocity((int) _vel);
+
+ double supplyCurrent = Math.abs(outPerc) * 30 * random(0.95, 1.05);
+ double statorCurrent = outPerc == 0 ? 0 : supplyCurrent / Math.abs(outPerc);
+ _falcon.getSimCollection().setSupplyCurrent(supplyCurrent);
+ _falcon.getSimCollection().setStatorCurrent(statorCurrent);
+
+ _falcon.getSimCollection().setBusVoltage(12 - outPerc * outPerc * 3 / 4 * random(0.95, 1.05));
+ }
+}
\ No newline at end of file
diff --git a/swervelib/simulation/ctre/TalonSRXSimProfile.java b/swervelib/simulation/ctre/TalonSRXSimProfile.java
new file mode 100644
index 0000000..299f33c
--- /dev/null
+++ b/swervelib/simulation/ctre/TalonSRXSimProfile.java
@@ -0,0 +1,88 @@
+package swervelib.simulation.ctre;
+
+import static swervelib.simulation.ctre.PhysicsSim.random;
+
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import swervelib.simulation.ctre.PhysicsSim.SimProfile;
+
+/**
+ * Holds information about a simulated TalonSRX.
+ */
+class TalonSRXSimProfile extends SimProfile
+{
+
+ private final TalonSRX _talon;
+ private final double _accelToFullTime;
+ private final double _fullVel;
+ private final boolean _sensorPhase;
+
+ /** The current position */
+ // private double _pos = 0;
+ /**
+ * The current velocity
+ */
+ private double _vel = 0;
+
+ /**
+ * Creates a new simulation profile for a TalonSRX device.
+ *
+ * @param talon The TalonSRX device
+ * @param accelToFullTime The time the motor takes to accelerate from 0 to full, in seconds
+ * @param fullVel The maximum motor velocity, in ticks per 100ms
+ * @param sensorPhase The phase of the TalonSRX sensors
+ */
+ public TalonSRXSimProfile(final TalonSRX talon, final double accelToFullTime, final double fullVel,
+ final boolean sensorPhase)
+ {
+ this._talon = talon;
+ this._accelToFullTime = accelToFullTime;
+ this._fullVel = fullVel;
+ this._sensorPhase = sensorPhase;
+ }
+
+ /**
+ * 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()
+ {
+ final double period = getPeriod();
+ final double accelAmount = _fullVel / _accelToFullTime * period / 1000;
+
+ /// DEVICE SPEED SIMULATION
+
+ double outPerc = _talon.getSimCollection().getMotorOutputLeadVoltage() / 12;
+ if (_sensorPhase)
+ {
+ outPerc *= -1;
+ }
+ // Calculate theoretical velocity with some randomness
+ double theoreticalVel = outPerc * _fullVel * random(0.95, 1);
+ // Simulate motor load
+ if (theoreticalVel > _vel + accelAmount)
+ {
+ _vel += accelAmount;
+ } else if (theoreticalVel < _vel - accelAmount)
+ {
+ _vel -= accelAmount;
+ } else
+ {
+ _vel += 0.9 * (theoreticalVel - _vel);
+ }
+ // _pos += _vel * period / 100;
+
+ /// SET SIM PHYSICS INPUTS
+
+ _talon.getSimCollection().addQuadraturePosition((int) (_vel * period / 100));
+ _talon.getSimCollection().setQuadratureVelocity((int) _vel);
+
+ double supplyCurrent = Math.abs(outPerc) * 30 * random(0.95, 1.05);
+ double statorCurrent = outPerc == 0 ? 0 : supplyCurrent / Math.abs(outPerc);
+ _talon.getSimCollection().setSupplyCurrent(supplyCurrent);
+ _talon.getSimCollection().setStatorCurrent(statorCurrent);
+
+ _talon.getSimCollection().setBusVoltage(12 - outPerc * outPerc * 3 / 4 * random(0.95, 1.05));
+ }
+}
diff --git a/swervelib/simulation/ctre/VictorSPXSimProfile.java b/swervelib/simulation/ctre/VictorSPXSimProfile.java
new file mode 100644
index 0000000..f1553ad
--- /dev/null
+++ b/swervelib/simulation/ctre/VictorSPXSimProfile.java
@@ -0,0 +1,41 @@
+package swervelib.simulation.ctre;
+
+
+import static swervelib.simulation.ctre.PhysicsSim.random;
+
+import com.ctre.phoenix.motorcontrol.can.VictorSPX;
+import swervelib.simulation.ctre.PhysicsSim.SimProfile;
+
+/**
+ * Holds information about a simulated VictorSPX.
+ */
+class VictorSPXSimProfile extends SimProfile
+{
+
+ public final VictorSPX _victor;
+
+ /**
+ * Creates a new simulation profile for a VictorSPX device.
+ *
+ * @param victor The VictorSPX device
+ */
+ public VictorSPXSimProfile(final VictorSPX victor)
+ {
+ this._victor = victor;
+ }
+
+ /**
+ * 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()
+ {
+ // final double period = getPeriod();
+
+ // Device voltage simulation
+ double outPerc = _victor.getSimCollection().getMotorOutputLeadVoltage() / 12;
+ _victor.getSimCollection().setBusVoltage(12 - outPerc * outPerc * 3 / 4 * random(0.95, 1.05));
+ }
+}