mirror of
https://github.com/BroncBotz3481/YAGSL
synced 2026-07-02 07:21:39 +00:00
Updated to 2024
This commit is contained in:
thenetworkgrinch
@@ -1,172 +0,0 @@
|
||||
package swervelib.simulation.ctre;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFX;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
|
||||
import com.ctre.phoenix.motorcontrol.can.VictorSPX;
|
||||
import java.util.ArrayList;
|
||||
|
||||
/**
|
||||
* Manages physics simulation for CTRE products.
|
||||
*/
|
||||
public class PhysicsSim
|
||||
{
|
||||
|
||||
private static PhysicsSim sim;
|
||||
private final ArrayList<SimProfile> _simProfiles = new ArrayList<SimProfile>();
|
||||
|
||||
/**
|
||||
* Gets the robot simulator instance.
|
||||
*
|
||||
* @return {@link PhysicsSim} instance.
|
||||
*/
|
||||
public static PhysicsSim getInstance()
|
||||
{
|
||||
if (sim == null)
|
||||
{
|
||||
sim = new PhysicsSim();
|
||||
}
|
||||
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.random() % 2 * Math.PI))
|
||||
+ (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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,92 +0,0 @@
|
||||
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.
|
||||
*
|
||||
* <p>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));
|
||||
}
|
||||
}
|
||||
@@ -1,92 +0,0 @@
|
||||
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.
|
||||
*
|
||||
* <p>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));
|
||||
}
|
||||
}
|
||||
@@ -1,41 +0,0 @@
|
||||
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.
|
||||
*
|
||||
* <p>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));
|
||||
}
|
||||
}
|
||||
@@ -1,4 +0,0 @@
|
||||
/**
|
||||
* CTRE Physics Simulator.
|
||||
*/
|
||||
package swervelib.simulation.ctre;
|
||||
Reference in New Issue
Block a user