Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-22 01:11:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Remove unit suffixes from variable names (#7529)

Browse Source 
* Move units into API docs instead because suffixes make user code verbose and hard to read
* Rename trackWidth to trackwidth
* Make ultrasonic classes use meters instead of a mix of m, cm, mm, ft,
  and inches
This commit is contained in:
Tyler Veness
2025-02-10 07:23:04 -08:00
committed by GitHub
parent 764ada9b66
commit ac1705ae2b
250 changed files with 2953 additions and 3584 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
View File

@@ -73,22 +73,13 @@ public class AddressableLED implements AutoCloseable {
* <p>By default, the driver is set up to drive WS2812B and WS2815, so nothing needs to be set for
* those.
*
* @param highTime0NanoSeconds high time for 0 bit (default 400ns)
* @param lowTime0NanoSeconds low time for 0 bit (default 900ns)
* @param highTime1NanoSeconds high time for 1 bit (default 900ns)
* @param lowTime1NanoSeconds low time for 1 bit (default 600ns)
* @param highTime0 high time for 0 bit in nanoseconds (default 400 ns)
* @param lowTime0 low time for 0 bit in nanoseconds (default 900 ns)
* @param highTime1 high time for 1 bit in nanoseconds (default 900 ns)
* @param lowTime1 low time for 1 bit in nanoseconds (default 600 ns)
*/
public void setBitTiming(
int highTime0NanoSeconds,
int lowTime0NanoSeconds,
int highTime1NanoSeconds,
int lowTime1NanoSeconds) {
AddressableLEDJNI.setBitTiming(
m_handle,
highTime0NanoSeconds,
lowTime0NanoSeconds,
highTime1NanoSeconds,
lowTime1NanoSeconds);
public void setBitTiming(int highTime0, int lowTime0, int highTime1, int lowTime1) {
AddressableLEDJNI.setBitTiming(m_handle, highTime0, lowTime0, highTime1, lowTime1);
}
/**
@@ -96,10 +87,10 @@ public class AddressableLED implements AutoCloseable {
*
* <p>The sync time is the time to hold output so LEDs enable. Default set for WS2812B and WS2815.
*
* @param syncTimeMicroSeconds the sync time (default 280us)
* @param syncTime the sync time in microseconds (default 280 μs)
*/
public void setSyncTime(int syncTimeMicroSeconds) {
AddressableLEDJNI.setSyncTime(m_handle, syncTimeMicroSeconds);
public void setSyncTime(int syncTime) {
AddressableLEDJNI.setSyncTime(m_handle, syncTime);
}
/**

6
wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
View File

@@ -83,10 +83,10 @@ public class DigitalOutput implements AutoCloseable, Sendable {
* <p>Send a single pulse on the digital output line where the pulse duration is specified in
* seconds. Maximum of 65535 microseconds.
*
* @param pulseLengthSeconds The pulse length in seconds
* @param pulseLength The pulse length in seconds
*/
public void pulse(final double pulseLengthSeconds) {
DIOJNI.pulse(m_handle, pulseLengthSeconds);
public void pulse(final double pulseLength) {
DIOJNI.pulse(m_handle, pulseLength);
}
/**

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
View File

@@ -1148,19 +1148,19 @@ public final class DriverStation {
/**
* Wait for a DS connection.
*
* @param timeoutSeconds timeout in seconds. 0 for infinite.
* @param timeout timeout in seconds. 0 for infinite.
* @return true if connected, false if timeout
*/
public static boolean waitForDsConnection(double timeoutSeconds) {
public static boolean waitForDsConnection(double timeout) {
int event = WPIUtilJNI.createEvent(true, false);
DriverStationJNI.provideNewDataEventHandle(event);
boolean result;
try {
if (timeoutSeconds == 0) {
if (timeout == 0) {
WPIUtilJNI.waitForObject(event);
result = true;
} else {
result = !WPIUtilJNI.waitForObjectTimeout(event, timeoutSeconds);
result = !WPIUtilJNI.waitForObjectTimeout(event, timeout);
}
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();

30
wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
View File

@@ -31,11 +31,11 @@ public class Notifier implements AutoCloseable {
// The time, in seconds, at which the callback should be called. Has the same
// zero as RobotController.getFPGATime().
private double m_expirationTimeSeconds;
private double m_expirationTime;
// If periodic, stores the callback period; if single, stores the time until
// the callback call.
private double m_periodSeconds;
// If periodic, stores the callback period in seconds; if single, stores the time until
// the callback call in seconds.
private double m_period;
// True if the callback is periodic
private boolean m_periodic;
@@ -75,7 +75,7 @@ public class Notifier implements AutoCloseable {
/** Update the alarm hardware to reflect the next alarm. */
private void updateAlarm() {
updateAlarm((long) (m_expirationTimeSeconds * 1e6));
updateAlarm((long) (m_expirationTime * 1e6));
}
/**
@@ -109,7 +109,7 @@ public class Notifier implements AutoCloseable {
try {
threadHandler = m_callback;
if (m_periodic) {
m_expirationTimeSeconds += m_periodSeconds;
m_expirationTime += m_period;
updateAlarm();
} else {
// Need to update the alarm to cause it to wait again
@@ -172,14 +172,14 @@ public class Notifier implements AutoCloseable {
/**
* Run the callback once after the given delay.
*
* @param delaySeconds Time in seconds to wait before the callback is called.
* @param delay Time in seconds to wait before the callback is called.
*/
public void startSingle(double delaySeconds) {
public void startSingle(double delay) {
m_processLock.lock();
try {
m_periodic = false;
m_periodSeconds = delaySeconds;
m_expirationTimeSeconds = RobotController.getFPGATime() * 1e-6 + delaySeconds;
m_period = delay;
m_expirationTime = RobotController.getFPGATime() * 1e-6 + delay;
updateAlarm();
} finally {
m_processLock.unlock();
@@ -192,15 +192,15 @@ public class Notifier implements AutoCloseable {
* <p>The user-provided callback should be written so that it completes before the next time it's
* scheduled to run.
*
* @param periodSeconds Period in seconds after which to to call the callback starting one period
* after the call to this method.
* @param period Period in seconds after which to to call the callback starting one period after
* the call to this method.
*/
public void startPeriodic(double periodSeconds) {
public void startPeriodic(double period) {
m_processLock.lock();
try {
m_periodic = true;
m_periodSeconds = periodSeconds;
m_expirationTimeSeconds = RobotController.getFPGATime() * 1e-6 + periodSeconds;
m_period = period;
m_expirationTime = RobotController.getFPGATime() * 1e-6 + period;
updateAlarm();
} finally {
m_processLock.unlock();

60
wpilibj/src/main/java/edu/wpi/first/wpilibj/SharpIR.java
View File

@@ -8,6 +8,7 @@ import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
@@ -28,47 +29,47 @@ public class SharpIR implements Sendable, AutoCloseable {
private final double m_A;
private final double m_B;
private final double m_minCM;
private final double m_maxCM;
private final double m_min; // m
private final double m_max; // m
/**
* Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring distances from 20cm to 150cm.
* Sharp GP2Y0A02YK0F is an analog IR sensor capable of measuring distances from 20 cm to 150 cm.
*
* @param channel Analog input channel the sensor is connected to
* @return sensor object
*/
public static SharpIR GP2Y0A02YK0F(int channel) {
return new SharpIR(channel, 62.28, -1.092, 20, 150.0);
return new SharpIR(channel, 62.28, -1.092, 0.2, 1.5);
}
/**
* Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring distances from 10cm to 80cm.
* Sharp GP2Y0A21YK0F is an analog IR sensor capable of measuring distances from 10 cm to 80 cm.
*
* @param channel Analog input channel the sensor is connected to
* @return sensor object
*/
public static SharpIR GP2Y0A21YK0F(int channel) {
return new SharpIR(channel, 26.449, -1.226, 10.0, 80.0);
return new SharpIR(channel, 26.449, -1.226, 0.1, 0.8);
}
/**
* Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring distances from 4cm to 30cm.
* Sharp GP2Y0A41SK0F is an analog IR sensor capable of measuring distances from 4 cm to 30 cm.
*
* @param channel Analog input channel the sensor is connected to
* @return sensor object
*/
public static SharpIR GP2Y0A41SK0F(int channel) {
return new SharpIR(channel, 12.354, -1.07, 4.0, 30.0);
return new SharpIR(channel, 12.354, -1.07, 0.04, 0.3);
}
/**
* Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring distances from 2cm to 15cm.
* Sharp GP2Y0A51SK0F is an analog IR sensor capable of measuring distances from 2 cm to 15 cm.
*
* @param channel Analog input channel the sensor is connected to
* @return sensor object
*/
public static SharpIR GP2Y0A51SK0F(int channel) {
return new SharpIR(channel, 5.2819, -1.161, 2.0, 15.0);
return new SharpIR(channel, 5.2819, -1.161, 0.02, 0.15);
}
/**
@@ -78,24 +79,24 @@ public class SharpIR implements Sendable, AutoCloseable {
* @param channel AnalogInput channel
* @param a Constant A
* @param b Constant B
* @param minCM Minimum distance to report in centimeters
* @param maxCM Maximum distance to report in centimeters
* @param min Minimum distance to report in meters
* @param max Maximum distance to report in meters
*/
@SuppressWarnings("this-escape")
public SharpIR(int channel, double a, double b, double minCM, double maxCM) {
public SharpIR(int channel, double a, double b, double min, double max) {
m_sensor = new AnalogInput(channel);
m_A = a;
m_B = b;
m_minCM = minCM;
m_maxCM = maxCM;
m_min = min;
m_max = max;
HAL.reportUsage("IO", channel, "SharpIR");
SendableRegistry.add(this, "SharpIR", channel);
m_simDevice = SimDevice.create("SharpIR", m_sensor.getChannel());
if (m_simDevice != null) {
m_simRange = m_simDevice.createDouble("Range (cm)", Direction.kInput, 0.0);
m_simRange = m_simDevice.createDouble("Range (m)", Direction.kInput, 0.0);
m_sensor.setSimDevice(m_simDevice);
}
}
@@ -123,37 +124,24 @@ public class SharpIR implements Sendable, AutoCloseable {
}
/**
* Get the range in inches from the distance sensor.
* Get the range in meters from the distance sensor.
*
* @return range in inches of the target returned by the sensor
* @return range in meters of the target returned by the sensor
*/
public double getRangeInches() {
return getRangeCM() / 2.54;
}
/**
* Get the range in centimeters from the distance sensor.
*
* @return range in centimeters of the target returned by the sensor
*/
public double getRangeCM() {
double distance;
public double getRange() {
if (m_simRange != null) {
distance = m_simRange.get();
return MathUtil.clamp(m_simRange.get(), m_min, m_max);
} else {
// Don't allow zero/negative values
var v = Math.max(m_sensor.getVoltage(), 0.00001);
distance = m_A * Math.pow(v, m_B);
}
// Always constrain output
return Math.max(Math.min(distance, m_maxCM), m_minCM);
return MathUtil.clamp(m_A * Math.pow(v, m_B) * 1e-2, m_min, m_max);
}
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Ultrasonic");
builder.addDoubleProperty("Value", this::getRangeInches, null);
builder.addDoubleProperty("Value", this::getRange, null);
}
}

6
wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
View File

@@ -125,11 +125,11 @@ public class Solenoid implements Sendable, AutoCloseable {
* <p>On the PH, the timing can be controlled in 0.001 second increments, with a maximum of 65.534
* seconds.
*
* @param durationSeconds The duration of the pulse in seconds.
* @param duration The duration of the pulse in seconds.
* @see #startPulse()
*/
public void setPulseDuration(double durationSeconds) {
long durationMS = (long) (durationSeconds * 1000);
public void setPulseDuration(double duration) {
long durationMS = (long) (duration * 1000);
m_module.setOneShotDuration(m_channel, (int) durationMS);
}

31
wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
View File

@@ -30,18 +30,18 @@ public class TimedRobot extends IterativeRobotBase {
* Construct a callback container.
*
* @param func The callback to run.
* @param startTimeUs The common starting point for all callback scheduling in microseconds.
* @param periodUs The period at which to run the callback in microseconds.
* @param offsetUs The offset from the common starting time in microseconds.
* @param startTime The common starting point for all callback scheduling in microseconds.
* @param period The period at which to run the callback in microseconds.
* @param offset The offset from the common starting time in microseconds.
*/
Callback(Runnable func, long startTimeUs, long periodUs, long offsetUs) {
Callback(Runnable func, long startTime, long period, long offset) {
this.func = func;
this.period = periodUs;
this.period = period;
this.expirationTime =
startTimeUs
+ offsetUs
startTime
+ offset
+ this.period
+ (RobotController.getFPGATime() - startTimeUs) / this.period * this.period;
+ (RobotController.getFPGATime() - startTime) / this.period * this.period;
}
@Override
@@ -177,10 +177,10 @@ public class TimedRobot extends IterativeRobotBase {
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param periodSeconds The period at which to run the callback in seconds.
* @param period The period at which to run the callback in seconds.
*/
public final void addPeriodic(Runnable callback, double periodSeconds) {
m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (periodSeconds * 1e6), 0));
public final void addPeriodic(Runnable callback, double period) {
m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (period * 1e6), 0));
}
/**
@@ -190,14 +190,13 @@ public class TimedRobot extends IterativeRobotBase {
* synchronously. Interactions between them are thread-safe.
*
* @param callback The callback to run.
* @param periodSeconds The period at which to run the callback in seconds.
* @param offsetSeconds The offset from the common starting time in seconds. This is useful for
* @param period The period at which to run the callback in seconds.
* @param offset The offset from the common starting time in seconds. This is useful for
* scheduling a callback in a different timeslot relative to TimedRobot.
*/
public final void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) {
public final void addPeriodic(Runnable callback, double period, double offset) {
m_callbacks.add(
new Callback(
callback, m_startTimeUs, (long) (periodSeconds * 1e6), (long) (offsetSeconds * 1e6)));
new Callback(callback, m_startTimeUs, (long) (period * 1e6), (long) (offset * 1e6)));
}
/**

48
wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
View File

@@ -19,13 +19,13 @@ import java.util.concurrent.locks.ReentrantLock;
*/
public class Watchdog implements Closeable, Comparable<Watchdog> {
// Used for timeout print rate-limiting
private static final long kMinPrintPeriodMicroS = (long) 1e6;
private static final long kMinPrintPeriod = (long) 1e6; // μs
private double m_startTimeSeconds;
private double m_timeoutSeconds;
private double m_expirationTimeSeconds;
private double m_startTime;
private double m_timeout;
private double m_expirationTime;
private final Runnable m_callback;
private double m_lastTimeoutPrintSeconds;
private double m_lastTimeoutPrint; // s
boolean m_isExpired;
@@ -46,11 +46,11 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
/**
* Watchdog constructor.
*
* @param timeoutSeconds The watchdog's timeout in seconds with microsecond resolution.
* @param timeout The watchdog's timeout in seconds with microsecond resolution.
* @param callback This function is called when the timeout expires.
*/
public Watchdog(double timeoutSeconds, Runnable callback) {
m_timeoutSeconds = timeoutSeconds;
public Watchdog(double timeout, Runnable callback) {
m_timeout = timeout;
m_callback = callback;
m_tracer = new Tracer();
}
@@ -63,19 +63,19 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
@Override
public boolean equals(Object obj) {
return obj instanceof Watchdog watchdog
&& Double.compare(m_expirationTimeSeconds, watchdog.m_expirationTimeSeconds) == 0;
&& Double.compare(m_expirationTime, watchdog.m_expirationTime) == 0;
}
@Override
public int hashCode() {
return Double.hashCode(m_expirationTimeSeconds);
return Double.hashCode(m_expirationTime);
}
@Override
public int compareTo(Watchdog rhs) {
// Elements with sooner expiration times are sorted as lesser. The head of
// Java's PriorityQueue is the least element.
return Double.compare(m_expirationTimeSeconds, rhs.m_expirationTimeSeconds);
return Double.compare(m_expirationTime, rhs.m_expirationTime);
}
/**
@@ -84,25 +84,25 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
* @return The time in seconds since the watchdog was last fed.
*/
public double getTime() {
return Timer.getFPGATimestamp() - m_startTimeSeconds;
return Timer.getFPGATimestamp() - m_startTime;
}
/**
* Sets the watchdog's timeout.
*
* @param timeoutSeconds The watchdog's timeout in seconds with microsecond resolution.
* @param timeout The watchdog's timeout in seconds with microsecond resolution.
*/
public void setTimeout(double timeoutSeconds) {
m_startTimeSeconds = Timer.getFPGATimestamp();
public void setTimeout(double timeout) {
m_startTime = Timer.getFPGATimestamp();
m_tracer.clearEpochs();
m_queueMutex.lock();
try {
m_timeoutSeconds = timeoutSeconds;
m_timeout = timeout;
m_isExpired = false;
m_watchdogs.remove(this);
m_expirationTimeSeconds = m_startTimeSeconds + m_timeoutSeconds;
m_expirationTime = m_startTime + m_timeout;
m_watchdogs.add(this);
updateAlarm();
} finally {
@@ -118,7 +118,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
public double getTimeout() {
m_queueMutex.lock();
try {
return m_timeoutSeconds;
return m_timeout;
} finally {
m_queueMutex.unlock();
}
@@ -168,7 +168,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
/** Enables the watchdog timer. */
public void enable() {
m_startTimeSeconds = Timer.getFPGATimestamp();
m_startTime = Timer.getFPGATimestamp();
m_tracer.clearEpochs();
m_queueMutex.lock();
@@ -176,7 +176,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
m_isExpired = false;
m_watchdogs.remove(this);
m_expirationTimeSeconds = m_startTimeSeconds + m_timeoutSeconds;
m_expirationTime = m_startTime + m_timeout;
m_watchdogs.add(this);
updateAlarm();
} finally {
@@ -212,7 +212,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
NotifierJNI.cancelNotifierAlarm(m_notifier);
} else {
NotifierJNI.updateNotifierAlarm(
m_notifier, (long) (m_watchdogs.peek().m_expirationTimeSeconds * 1e6));
m_notifier, (long) (m_watchdogs.peek().m_expirationTime * 1e6));
}
}
@@ -241,11 +241,11 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
Watchdog watchdog = m_watchdogs.poll();
double now = curTime * 1e-6;
if (now - watchdog.m_lastTimeoutPrintSeconds > kMinPrintPeriodMicroS) {
watchdog.m_lastTimeoutPrintSeconds = now;
if (now - watchdog.m_lastTimeoutPrint > kMinPrintPeriod) {
watchdog.m_lastTimeoutPrint = now;
if (!watchdog.m_suppressTimeoutMessage) {
DriverStation.reportWarning(
String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeoutSeconds), false);
String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout), false);
}
}

113
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DCMotorSim.java
View File

@@ -4,19 +4,11 @@
package edu.wpi.first.wpilibj.simulation;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.Angle;
import edu.wpi.first.units.measure.AngularAcceleration;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.wpilibj.RobotController;
/** Represents a simulated DC motor mechanism. */
@@ -27,8 +19,8 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
// The gearing from the motors to the output.
private final double m_gearing;
// The moment of inertia for the DC motor mechanism.
private final double m_jKgMetersSquared;
// The moment of inertia for the DC motor mechanism in kg-m².
private final double m_j;
/**
* Creates a simulated DC motor mechanism.
@@ -64,36 +56,36 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
//
// B = GKₜ/(RJ)
// J = GKₜ/(RB)
m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(1, 1) / plant.getB(1, 0);
m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(1, 0));
m_gearing = -gearbox.Kv * plant.getA(1, 1) / plant.getB(1, 0);
m_j = m_gearing * gearbox.Kt / (gearbox.R * plant.getB(1, 0));
}
/**
* Sets the state of the DC motor.
*
* @param angularPositionRad The new position in radians.
* @param angularVelocityRadPerSec The new velocity in radians per second.
* @param angularPosition The new position in radians.
* @param angularVelocity The new velocity in radians per second.
*/
public void setState(double angularPositionRad, double angularVelocityRadPerSec) {
setState(VecBuilder.fill(angularPositionRad, angularVelocityRadPerSec));
public void setState(double angularPosition, double angularVelocity) {
setState(VecBuilder.fill(angularPosition, angularVelocity));
}
/**
* Sets the DC motor's angular position.
*
* @param angularPositionRad The new position in radians.
* @param angularPosition The new position in radians.
*/
public void setAngle(double angularPositionRad) {
setState(angularPositionRad, getAngularVelocityRadPerSec());
public void setAngle(double angularPosition) {
setState(angularPosition, getAngularVelocity());
}
/**
* Sets the DC motor's angular velocity.
*
* @param angularVelocityRadPerSec The new velocity in radians per second.
* @param angularVelocity The new velocity in radians per second.
*/
public void setAngularVelocity(double angularVelocityRadPerSec) {
setState(getAngularPositionRad(), angularVelocityRadPerSec);
public void setAngularVelocity(double angularVelocity) {
setState(getAngularPosition(), angularVelocity);
}
/**
@@ -108,10 +100,10 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
/**
* Returns the moment of inertia of the DC motor.
*
* @return The DC motor's moment of inertia.
* @return The DC motor's moment of inertia in kg-m².
*/
public double getJKgMetersSquared() {
return m_jKgMetersSquared;
public double getJ() {
return m_j;
}
/**
@@ -126,91 +118,46 @@ public class DCMotorSim extends LinearSystemSim<N2, N1, N2> {
/**
* Returns the DC motor's position.
*
* @return The DC motor's position.
* @return The DC motor's position in meters.
*/
public double getAngularPositionRad() {
public double getAngularPosition() {
return getOutput(0);
}
/**
* Returns the DC motor's position in rotations.
*
* @return The DC motor's position in rotations.
*/
public double getAngularPositionRotations() {
return Units.radiansToRotations(getAngularPositionRad());
}
/**
* Returns the DC motor's position.
*
* @return The DC motor's position
*/
public Angle getAngularPosition() {
return Radians.of(getAngularPositionRad());
}
/**
* Returns the DC motor's velocity.
*
* @return The DC motor's velocity.
*/
public double getAngularVelocityRadPerSec() {
public double getAngularVelocity() {
return getOutput(1);
}
/**
* Returns the DC motor's velocity in RPM.
*
* @return The DC motor's velocity in RPM.
*/
public double getAngularVelocityRPM() {
return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec());
}
/**
* Returns the DC motor's velocity.
*
* @return The DC motor's velocity
*/
public AngularVelocity getAngularVelocity() {
return RadiansPerSecond.of(getAngularVelocityRadPerSec());
}
/**
* Returns the DC motor's acceleration in Radians Per Second Squared.
*
* @return The DC motor's acceleration in Radians Per Second Squared.
*/
public double getAngularAccelerationRadPerSecSq() {
var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
return acceleration.get(1, 0);
}
/**
* Returns the DC motor's acceleration.
*
* @return The DC motor's acceleration.
* @return The DC motor's acceleration in rad/s².
*/
public AngularAcceleration getAngularAcceleration() {
return RadiansPerSecondPerSecond.of(getAngularAccelerationRadPerSecSq());
public double getAngularAcceleration() {
var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
return acceleration.get(1, 0);
}
/**
* Returns the DC motor's torque in Newton-Meters.
* Returns the DC motor's torque in.
*
* @return The DC motor's torque in Newton-Meters.
* @return The DC motor's torque in Newton-meters.
*/
public double getTorqueNewtonMeters() {
return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared;
public double getTorque() {
return getAngularAcceleration() * m_j;
}
/**
* Returns the DC motor's current draw.
*
* @return The DC motor's current draw.
* @return The DC motor's current draw in amps.
*/
public double getCurrentDrawAmps() {
public double getCurrentDraw() {
// I = V / R - omega / (Kv * R)
// Reductions are output over input, so a reduction of 2:1 means the motor is spinning
// 2x faster than the output

92
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
View File

@@ -42,7 +42,7 @@ public class DifferentialDrivetrainSim {
private final double m_originalGearing;
private final Matrix<N7, N1> m_measurementStdDevs;
private double m_currentGearing;
private final double m_wheelRadiusMeters;
private final double m_wheelRadius;
private Matrix<N2, N1> m_u;
private Matrix<N7, N1> m_x;
@@ -57,10 +57,10 @@ public class DifferentialDrivetrainSim {
* @param driveMotor A {@link DCMotor} representing the left side of the drivetrain.
* @param gearing The gearing ratio between motor and wheel, as output over input. This must be
* the same ratio as the ratio used to identify or create the drivetrainPlant.
* @param jKgMetersSquared The moment of inertia of the drivetrain about its center.
* @param massKg The mass of the drivebase.
* @param wheelRadiusMeters The radius of the wheels on the drivetrain.
* @param trackWidthMeters The robot's track width, or distance between left and right wheels.
* @param j The moment of inertia of the drivetrain about its center in kg-m².
* @param mass The mass of the drivebase in kg.
* @param wheelRadius The radius of the wheels on the drivetrain in meters.
* @param trackwidth The robot's trackwidth, or distance between left and right wheels, in meters.
* @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
* left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
* desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
@@ -70,23 +70,18 @@ public class DifferentialDrivetrainSim {
public DifferentialDrivetrainSim(
DCMotor driveMotor,
double gearing,
double jKgMetersSquared,
double massKg,
double wheelRadiusMeters,
double trackWidthMeters,
double j,
double mass,
double wheelRadius,
double trackwidth,
Matrix<N7, N1> measurementStdDevs) {
this(
LinearSystemId.createDrivetrainVelocitySystem(
driveMotor,
massKg,
wheelRadiusMeters,
trackWidthMeters / 2.0,
jKgMetersSquared,
gearing),
driveMotor, mass, wheelRadius, trackwidth / 2.0, j, gearing),
driveMotor,
gearing,
trackWidthMeters,
wheelRadiusMeters,
trackwidth,
wheelRadius,
measurementStdDevs);
}
@@ -102,9 +97,9 @@ public class DifferentialDrivetrainSim {
* @param driveMotor A {@link DCMotor} representing the drivetrain.
* @param gearing The gearingRatio ratio of the robot, as output over input. This must be the same
* ratio as the ratio used to identify or create the drivetrainPlant.
* @param trackWidthMeters The distance between the two sides of the drivetrain. Can be found with
* SysId.
* @param wheelRadiusMeters The radius of the wheels on the drivetrain, in meters.
* @param trackwidth The distance between the two sides of the drivetrain in meters. Can be found
* with SysId.
* @param wheelRadius The radius of the wheels on the drivetrain, in meters.
* @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
* left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
* desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
@@ -115,15 +110,15 @@ public class DifferentialDrivetrainSim {
LinearSystem<N2, N2, N2> plant,
DCMotor driveMotor,
double gearing,
double trackWidthMeters,
double wheelRadiusMeters,
double trackwidth,
double wheelRadius,
Matrix<N7, N1> measurementStdDevs) {
this.m_plant = plant;
this.m_rb = trackWidthMeters / 2.0;
this.m_rb = trackwidth / 2.0;
this.m_motor = driveMotor;
this.m_originalGearing = gearing;
this.m_measurementStdDevs = measurementStdDevs;
m_wheelRadiusMeters = wheelRadiusMeters;
m_wheelRadius = wheelRadius;
m_currentGearing = m_originalGearing;
m_x = new Matrix<>(Nat.N7(), Nat.N1());
@@ -145,10 +140,10 @@ public class DifferentialDrivetrainSim {
/**
* Update the drivetrain states with the current time difference.
*
* @param dtSeconds the time difference
* @param dt the time difference in seconds
*/
public void update(double dtSeconds) {
m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dtSeconds);
public void update(double dt) {
m_x = NumericalIntegration.rkdp(this::getDynamics, m_x, m_u, dt);
m_y = m_x;
if (m_measurementStdDevs != null) {
m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
@@ -195,38 +190,38 @@ public class DifferentialDrivetrainSim {
}
/**
* Get the right encoder position in meters.
* Get the right encoder position.
*
* @return The encoder position.
* @return The encoder position in meters.
*/
public double getRightPositionMeters() {
public double getRightPosition() {
return getOutput(State.kRightPosition);
}
/**
* Get the right encoder velocity in meters per second.
* Get the right encoder velocity.
*
* @return The encoder velocity.
* @return The encoder velocity in meters per second.
*/
public double getRightVelocityMetersPerSecond() {
public double getRightVelocity() {
return getOutput(State.kRightVelocity);
}
/**
* Get the left encoder position in meters.
* Get the left encoder position.
*
* @return The encoder position.
* @return The encoder position in meters.
*/
public double getLeftPositionMeters() {
public double getLeftPosition() {
return getOutput(State.kLeftPosition);
}
/**
* Get the left encoder velocity in meters per second.
* Get the left encoder velocity.
*
* @return The encoder velocity.
* @return The encoder velocity in meters per second.
*/
public double getLeftVelocityMetersPerSecond() {
public double getLeftVelocity() {
return getOutput(State.kLeftVelocity);
}
@@ -235,9 +230,9 @@ public class DifferentialDrivetrainSim {
*
* @return the drivetrain's left side current draw, in amps
*/
public double getLeftCurrentDrawAmps() {
public double getLeftCurrentDraw() {
return m_motor.getCurrent(
getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadiusMeters, m_u.get(0, 0))
getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadius, m_u.get(0, 0))
* Math.signum(m_u.get(0, 0));
}
@@ -246,9 +241,9 @@ public class DifferentialDrivetrainSim {
*
* @return the drivetrain's right side current draw, in amps
*/
public double getRightCurrentDrawAmps() {
public double getRightCurrentDraw() {
return m_motor.getCurrent(
getState(State.kRightVelocity) * m_currentGearing / m_wheelRadiusMeters, m_u.get(1, 0))
getState(State.kRightVelocity) * m_currentGearing / m_wheelRadius, m_u.get(1, 0))
* Math.signum(m_u.get(1, 0));
}
@@ -257,8 +252,8 @@ public class DifferentialDrivetrainSim {
*
* @return the current draw, in amps
*/
public double getCurrentDrawAmps() {
return getLeftCurrentDrawAmps() + getRightCurrentDrawAmps();
public double getCurrentDraw() {
return getLeftCurrentDraw() + getRightCurrentDraw();
}
/**
@@ -472,8 +467,7 @@ public class DifferentialDrivetrainSim {
* @param motor The motors installed in the bot.
* @param gearing The gearing reduction used.
* @param wheelSize The wheel size.
* @param jKgMetersSquared The moment of inertia of the drivebase. This can be calculated using
* SysId.
* @param j The moment of inertia of the drivebase in kg-m². This can be calculated using SysId.
* @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
* left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
* desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
@@ -485,12 +479,12 @@ public class DifferentialDrivetrainSim {
KitbotMotor motor,
KitbotGearing gearing,
KitbotWheelSize wheelSize,
double jKgMetersSquared,
double j,
Matrix<N7, N1> measurementStdDevs) {
return new DifferentialDrivetrainSim(
motor.value,
gearing.value,
jKgMetersSquared,
j,
Units.lbsToKilograms(60),
wheelSize.value / 2.0,
Units.inchesToMeters(26),

113
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
View File

@@ -36,10 +36,10 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* {@link edu.wpi.first.math.system.plant.LinearSystemId#createElevatorSystem(DCMotor, double,
* double, double)}.
* @param gearbox The type of and number of motors in the elevator gearbox.
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param minHeight The min allowable height of the elevator in meters.
* @param maxHeight The max allowable height of the elevator in meters.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeightMeters The starting height of the elevator.
* @param startingHeight The starting height of the elevator in meters.
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
* noise is desired. If present must have 1 element for position.
*/
@@ -47,18 +47,18 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
public ElevatorSim(
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double minHeightMeters,
double maxHeightMeters,
double minHeight,
double maxHeight,
boolean simulateGravity,
double startingHeightMeters,
double startingHeight,
double... measurementStdDevs) {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_minHeight = minHeightMeters;
m_maxHeight = maxHeightMeters;
m_minHeight = minHeight;
m_maxHeight = maxHeight;
m_simulateGravity = simulateGravity;
setState(startingHeightMeters, 0);
setState(startingHeight, 0);
}
/**
@@ -67,10 +67,10 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* @param kV The velocity gain.
* @param kA The acceleration gain.
* @param gearbox The type of and number of motors in the elevator gearbox.
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param minHeight The min allowable height of the elevator in meters.
* @param maxHeight The max allowable height of the elevator in meters.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeightMeters The starting height of the elevator.
* @param startingHeight The starting height of the elevator in meters.
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
* noise is desired. If present must have 1 element for position.
*/
@@ -78,18 +78,18 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
double kV,
double kA,
DCMotor gearbox,
double minHeightMeters,
double maxHeightMeters,
double minHeight,
double maxHeight,
boolean simulateGravity,
double startingHeightMeters,
double startingHeight,
double... measurementStdDevs) {
this(
LinearSystemId.identifyPositionSystem(kV, kA),
gearbox,
minHeightMeters,
maxHeightMeters,
minHeight,
maxHeight,
simulateGravity,
startingHeightMeters,
startingHeight,
measurementStdDevs);
}
@@ -98,32 +98,32 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
*
* @param gearbox The type of and number of motors in the elevator gearbox.
* @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
* @param carriageMassKg The mass of the elevator carriage.
* @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
* @param minHeightMeters The min allowable height of the elevator.
* @param maxHeightMeters The max allowable height of the elevator.
* @param carriageMass The mass of the elevator carriage in kg.
* @param drumRadius The radius of the drum that the elevator spool is wrapped around in meters.
* @param minHeight The min allowable height of the elevator in meters.
* @param maxHeight The max allowable height of the elevator in meters.
* @param simulateGravity Whether gravity should be simulated or not.
* @param startingHeightMeters The starting height of the elevator.
* @param startingHeight The starting height of the elevator in meters.
* @param measurementStdDevs The standard deviations of the measurements. Can be omitted if no
* noise is desired. If present must have 1 element for position.
*/
public ElevatorSim(
DCMotor gearbox,
double gearing,
double carriageMassKg,
double drumRadiusMeters,
double minHeightMeters,
double maxHeightMeters,
double carriageMass,
double drumRadius,
double minHeight,
double maxHeight,
boolean simulateGravity,
double startingHeightMeters,
double startingHeight,
double... measurementStdDevs) {
this(
LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
LinearSystemId.createElevatorSystem(gearbox, carriageMass, drumRadius, gearing),
gearbox,
minHeightMeters,
maxHeightMeters,
minHeight,
maxHeight,
simulateGravity,
startingHeightMeters,
startingHeight,
measurementStdDevs);
}
@@ -131,33 +131,31 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* Sets the elevator's state. The new position will be limited between the minimum and maximum
* allowed heights.
*
* @param positionMeters The new position in meters.
* @param velocityMetersPerSecond New velocity in meters per second.
* @param position The new position in meters.
* @param velocity New velocity in meters per second.
*/
public final void setState(double positionMeters, double velocityMetersPerSecond) {
setState(
VecBuilder.fill(
MathUtil.clamp(positionMeters, m_minHeight, m_maxHeight), velocityMetersPerSecond));
public final void setState(double position, double velocity) {
setState(VecBuilder.fill(MathUtil.clamp(position, m_minHeight, m_maxHeight), velocity));
}
/**
* Returns whether the elevator would hit the lower limit.
*
* @param elevatorHeightMeters The elevator height.
* @param elevatorHeight The elevator height in meters.
* @return Whether the elevator would hit the lower limit.
*/
public boolean wouldHitLowerLimit(double elevatorHeightMeters) {
return elevatorHeightMeters <= this.m_minHeight;
public boolean wouldHitLowerLimit(double elevatorHeight) {
return elevatorHeight <= this.m_minHeight;
}
/**
* Returns whether the elevator would hit the upper limit.
*
* @param elevatorHeightMeters The elevator height.
* @param elevatorHeight The elevator height in meters.
* @return Whether the elevator would hit the upper limit.
*/
public boolean wouldHitUpperLimit(double elevatorHeightMeters) {
return elevatorHeightMeters >= this.m_maxHeight;
public boolean wouldHitUpperLimit(double elevatorHeight) {
return elevatorHeight >= this.m_maxHeight;
}
/**
@@ -166,7 +164,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* @return Whether the elevator has hit the lower limit.
*/
public boolean hasHitLowerLimit() {
return wouldHitLowerLimit(getPositionMeters());
return wouldHitLowerLimit(getPosition());
}
/**
@@ -175,43 +173,42 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
* @return Whether the elevator has hit the upper limit.
*/
public boolean hasHitUpperLimit() {
return wouldHitUpperLimit(getPositionMeters());
return wouldHitUpperLimit(getPosition());
}
/**
* Returns the position of the elevator.
*
* @return The position of the elevator.
* @return The position of the elevator in meters.
*/
public double getPositionMeters() {
public double getPosition() {
return getOutput(0);
}
/**
* Returns the velocity of the elevator.
*
* @return The velocity of the elevator.
* @return The velocity of the elevator in meters per second.
*/
public double getVelocityMetersPerSecond() {
public double getVelocity() {
return getOutput(1);
}
/**
* Returns the elevator current draw.
*
* @return The elevator current draw.
* @return The elevator current draw in amps.
*/
public double getCurrentDrawAmps() {
public double getCurrentDraw() {
// I = V / R - omega / (Kv * R)
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
// spinning 10x faster than the output
// v = r w, so w = v/r
double kA = 1 / m_plant.getB().get(1, 0);
double kV = -m_plant.getA().get(1, 1) * kA;
double motorVelocityRadPerSec = m_x.get(1, 0) * kV * m_gearbox.KvRadPerSecPerVolt;
double motorVelocity = m_x.get(1, 0) * kV * m_gearbox.Kv;
var appliedVoltage = m_u.get(0, 0);
return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage)
* Math.signum(appliedVoltage);
return m_gearbox.getCurrent(motorVelocity, appliedVoltage) * Math.signum(appliedVoltage);
}
/**
@@ -229,10 +226,10 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
*
* @param currentXhat The current state estimate.
* @param u The system inputs (voltage).
* @param dtSeconds The time difference between controller updates.
* @param dt The time difference between controller updates in seconds.
*/
@Override
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dt) {
// Calculate updated x-hat from Runge-Kutta.
var updatedXhat =
NumericalIntegration.rkdp(
@@ -245,7 +242,7 @@ public class ElevatorSim extends LinearSystemSim<N2, N1, N2> {
},
currentXhat,
u,
dtSeconds);
dt);
// We check for collisions after updating x-hat.
if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {

79
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
View File

@@ -4,17 +4,11 @@
package edu.wpi.first.wpilibj.simulation;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.units.measure.AngularAcceleration;
import edu.wpi.first.units.measure.AngularVelocity;
import edu.wpi.first.wpilibj.RobotController;
/** Represents a simulated flywheel mechanism. */
@@ -26,7 +20,7 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
private final double m_gearing;
// The moment of inertia for the flywheel mechanism.
private final double m_jKgMetersSquared;
private final double m_j;
/**
* Creates a simulated flywheel mechanism.
@@ -60,17 +54,17 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
//
// B = GKₜ/(RJ)
// J = GKₜ/(RB)
m_gearing = -gearbox.KvRadPerSecPerVolt * plant.getA(0, 0) / plant.getB(0, 0);
m_jKgMetersSquared = m_gearing * gearbox.KtNMPerAmp / (gearbox.rOhms * plant.getB(0, 0));
m_gearing = -gearbox.Kv * plant.getA(0, 0) / plant.getB(0, 0);
m_j = m_gearing * gearbox.Kt / (gearbox.R * plant.getB(0, 0));
}
/**
* Sets the flywheel's angular velocity.
*
* @param velocityRadPerSec The new velocity in radians per second.
* @param velocity The new velocity in radians per second.
*/
public void setAngularVelocity(double velocityRadPerSec) {
setState(VecBuilder.fill(velocityRadPerSec));
public void setAngularVelocity(double velocity) {
setState(VecBuilder.fill(velocity));
}
/**
@@ -83,12 +77,12 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
}
/**
* Returns the moment of inertia in kilograms meters squared.
* Returns the moment of inertia.
*
* @return The flywheel's moment of inertia.
* @return The flywheel's moment of inertia in kg-m².
*/
public double getJKgMetersSquared() {
return m_jKgMetersSquared;
public double getJ() {
return m_j;
}
/**
@@ -100,67 +94,40 @@ public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
return m_gearbox;
}
/**
* Returns the flywheel's velocity in Radians Per Second.
*
* @return The flywheel's velocity in Radians Per Second.
*/
public double getAngularVelocityRadPerSec() {
return getOutput(0);
}
/**
* Returns the flywheel's velocity in RPM.
*
* @return The flywheel's velocity in RPM.
*/
public double getAngularVelocityRPM() {
return Units.radiansPerSecondToRotationsPerMinute(getAngularVelocityRadPerSec());
}
/**
* Returns the flywheel's velocity.
*
* @return The flywheel's velocity
* @return The flywheel's velocity in rad/s.
*/
public AngularVelocity getAngularVelocity() {
return RadiansPerSecond.of(getAngularVelocityRadPerSec());
}
/**
* Returns the flywheel's acceleration in Radians Per Second Squared.
*
* @return The flywheel's acceleration in Radians Per Second Squared.
*/
public double getAngularAccelerationRadPerSecSq() {
var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
return acceleration.get(0, 0);
public double getAngularVelocity() {
return getOutput(0);
}
/**
* Returns the flywheel's acceleration.
*
* @return The flywheel's acceleration.
* @return The flywheel's acceleration in rad/s².
*/
public AngularAcceleration getAngularAcceleration() {
return RadiansPerSecondPerSecond.of(getAngularAccelerationRadPerSecSq());
public double getAngularAcceleration() {
var acceleration = (m_plant.getA().times(m_x)).plus(m_plant.getB().times(m_u));
return acceleration.get(0, 0);
}
/**
* Returns the flywheel's torque in Newton-Meters.
* Returns the flywheel's torque.
*
* @return The flywheel's torque in Newton-Meters.
* @return The flywheel's torque in Newton-meters.
*/
public double getTorqueNewtonMeters() {
return getAngularAccelerationRadPerSecSq() * m_jKgMetersSquared;
public double getTorque() {
return getAngularAcceleration() * m_j;
}
/**
* Returns the flywheel's current draw.
*
* @return The flywheel's current draw.
* @return The flywheel's current draw in amps.
*/
public double getCurrentDrawAmps() {
public double getCurrentDraw() {
// I = V / R - omega / (Kv * R)
// Reductions are output over input, so a reduction of 2:1 means the motor is spinning
// 2x faster than the flywheel

12
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
View File

@@ -72,11 +72,11 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
/**
* Updates the simulation.
*
* @param dtSeconds The time between updates.
* @param dt The time between updates in seconds.
*/
public void update(double dtSeconds) {
public void update(double dt) {
// Update X. By default, this is the linear system dynamics X = Ax + Bu
m_x = updateX(m_x, m_u, dtSeconds);
m_x = updateX(m_x, m_u, dt);
// y = cx + du
m_y = m_plant.calculateY(m_x, m_u);
@@ -171,12 +171,12 @@ public class LinearSystemSim<States extends Num, Inputs extends Num, Outputs ext
*
* @param currentXhat The current state estimate.
* @param u The system inputs (usually voltage).
* @param dtSeconds The time difference between controller updates.
* @param dt The time difference between controller updates in seconds.
* @return The new state.
*/
protected Matrix<States, N1> updateX(
Matrix<States, N1> currentXhat, Matrix<Inputs, N1> u, double dtSeconds) {
return m_plant.calculateX(currentXhat, u, dtSeconds);
Matrix<States, N1> currentXhat, Matrix<Inputs, N1> u, double dt) {
return m_plant.calculateX(currentXhat, u, dt);
}
/**

20
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SharpIRSim.java
View File

@@ -5,7 +5,6 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.SharpIR;
/** Simulation class for Sharp IR sensors. */
@@ -28,24 +27,15 @@ public class SharpIRSim {
*/
public SharpIRSim(int channel) {
SimDeviceSim simDevice = new SimDeviceSim("SharpIR", channel);
m_simRange = simDevice.getDouble("Range (cm)");
m_simRange = simDevice.getDouble("Range (m)");
}
/**
* Set the range in inches returned by the distance sensor.
* Set the range in meters returned by the distance sensor.
*
* @param inches range in inches of the target returned by the sensor
* @param range range in meters of the target returned by the sensor
*/
public void setRangeInches(double inches) {
m_simRange.set(Units.inchesToMeters(inches) * 100.0);
}
/**
* Set the range in centimeters returned by the distance sensor.
*
* @param cm range in centimeters of the target returned by the sensor
*/
public void setRangeCm(double cm) {
m_simRange.set(cm);
public void setRange(double range) {
m_simRange.set(range);
}
}

12
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java
View File

@@ -65,18 +65,18 @@ public final class SimHooks {
/**
* Advance the simulator time and wait for all notifiers to run.
*
* @param deltaSeconds the amount to advance (in seconds)
* @param delta the amount to advance in seconds
*/
public static void stepTiming(double deltaSeconds) {
SimulatorJNI.stepTiming((long) (deltaSeconds * 1e6));
public static void stepTiming(double delta) {
SimulatorJNI.stepTiming((long) (delta * 1e6));
}
/**
* Advance the simulator time and return immediately.
*
* @param deltaSeconds the amount to advance (in seconds)
* @param delta the amount to advance in seconds
*/
public static void stepTimingAsync(double deltaSeconds) {
SimulatorJNI.stepTimingAsync((long) (deltaSeconds * 1e6));
public static void stepTimingAsync(double delta) {
SimulatorJNI.stepTimingAsync((long) (delta * 1e6));
}
}

54
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
View File

@@ -24,7 +24,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
private final double m_gearing;
// The length of the arm.
private final double m_armLenMeters;
private final double m_armLength;
// The minimum angle that the arm is capable of.
private final double m_minAngle;
@@ -43,7 +43,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
* double, double)}.
* @param gearbox The type of and number of motors in the arm gearbox.
* @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
* @param armLengthMeters The length of the arm.
* @param armLength The length of the arm in meters.
* @param minAngleRads The minimum angle that the arm is capable of.
* @param maxAngleRads The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
@@ -56,7 +56,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
LinearSystem<N2, N1, N2> plant,
DCMotor gearbox,
double gearing,
double armLengthMeters,
double armLength,
double minAngleRads,
double maxAngleRads,
boolean simulateGravity,
@@ -65,7 +65,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
super(plant, measurementStdDevs);
m_gearbox = gearbox;
m_gearing = gearing;
m_armLenMeters = armLengthMeters;
m_armLength = armLength;
m_minAngle = minAngleRads;
m_maxAngle = maxAngleRads;
m_simulateGravity = simulateGravity;
@@ -78,8 +78,8 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
*
* @param gearbox The type of and number of motors in the arm gearbox.
* @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
* @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software.
* @param armLengthMeters The length of the arm.
* @param j The moment of inertia of the arm in kg-m²; can be calculated from CAD software.
* @param armLength The length of the arm in meters.
* @param minAngleRads The minimum angle that the arm is capable of.
* @param maxAngleRads The maximum angle that the arm is capable of.
* @param simulateGravity Whether gravity should be simulated or not.
@@ -90,18 +90,18 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
public SingleJointedArmSim(
DCMotor gearbox,
double gearing,
double jKgMetersSquared,
double armLengthMeters,
double j,
double armLength,
double minAngleRads,
double maxAngleRads,
boolean simulateGravity,
double startingAngleRads,
double... measurementStdDevs) {
this(
LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
LinearSystemId.createSingleJointedArmSystem(gearbox, j, gearing),
gearbox,
gearing,
armLengthMeters,
armLength,
minAngleRads,
maxAngleRads,
simulateGravity,
@@ -147,7 +147,7 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
* @return Whether the arm has hit the lower limit.
*/
public boolean hasHitLowerLimit() {
return wouldHitLowerLimit(getAngleRads());
return wouldHitLowerLimit(getAngle());
}
/**
@@ -156,33 +156,33 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
* @return Whether the arm has hit the upper limit.
*/
public boolean hasHitUpperLimit() {
return wouldHitUpperLimit(getAngleRads());
return wouldHitUpperLimit(getAngle());
}
/**
* Returns the current arm angle.
*
* @return The current arm angle.
* @return The current arm angle in radians.
*/
public double getAngleRads() {
public double getAngle() {
return getOutput(0);
}
/**
* Returns the current arm velocity.
*
* @return The current arm velocity.
* @return The current arm velocity in radians per second.
*/
public double getVelocityRadPerSec() {
public double getVelocity() {
return getOutput(1);
}
/**
* Returns the arm current draw.
*
* @return The arm current draw.
* @return The arm current draw in amps.
*/
public double getCurrentDrawAmps() {
public double getCurrentDraw() {
// Reductions are greater than 1, so a reduction of 10:1 would mean the motor is
// spinning 10x faster than the output
var motorVelocity = m_x.get(1, 0) * m_gearing;
@@ -202,12 +202,12 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
/**
* Calculates a rough estimate of the moment of inertia of an arm given its length and mass.
*
* @param lengthMeters The length of the arm.
* @param massKg The mass of the arm.
* @return The calculated moment of inertia.
* @param length The length of the arm in m.
* @param mass The mass of the arm in kg.
* @return The calculated moment of inertia in kg-m².
*/
public static double estimateMOI(double lengthMeters, double massKg) {
return 1.0 / 3.0 * massKg * lengthMeters * lengthMeters;
public static double estimateMOI(double length, double mass) {
return 1.0 / 3.0 * mass * length * length;
}
/**
@@ -215,10 +215,10 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
*
* @param currentXhat The current state estimate.
* @param u The system inputs (voltage).
* @param dtSeconds The time difference between controller updates.
* @param dt The time difference between controller updates in seconds.
*/
@Override
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dt) {
// The torque on the arm is given by τ = F⋅r, where F is the force applied by
// gravity and r the distance from pivot to center of mass. Recall from
// dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is
@@ -248,14 +248,14 @@ public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N2> {
(Matrix<N2, N1> x, Matrix<N1, N1> _u) -> {
Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(_u));
if (m_simulateGravity) {
double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLenMeters;
double alphaGrav = 3.0 / 2.0 * -9.8 * Math.cos(x.get(0, 0)) / m_armLength;
xdot = xdot.plus(VecBuilder.fill(0, alphaGrav));
}
return xdot;
},
currentXhat,
u,
dtSeconds);
dt);
// We check for collision after updating xhat
if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java
View File

@@ -58,12 +58,12 @@ public class Field2d implements NTSendable, AutoCloseable {
/**
* Set the robot pose from x, y, and rotation.
*
* @param xMeters X location, in meters
* @param yMeters Y location, in meters
* @param x X location, in meters
* @param y Y location, in meters
* @param rotation rotation
*/
public synchronized void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
m_objects.get(0).setPose(xMeters, yMeters, rotation);
public synchronized void setRobotPose(double x, double y, Rotation2d rotation) {
m_objects.get(0).setPose(x, y, rotation);
}
/**

10
wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
View File

@@ -43,12 +43,12 @@ public class FieldObject2d implements AutoCloseable {
/**
* Set the pose from x, y, and rotation.
*
* @param xMeters X location, in meters
* @param yMeters Y location, in meters
* @param x X location, in meters
* @param y Y location, in meters
* @param rotation rotation
*/
public synchronized void setPose(double xMeters, double yMeters, Rotation2d rotation) {
setPose(new Pose2d(xMeters, yMeters, rotation));
public synchronized void setPose(double x, double y, Rotation2d rotation) {
setPose(new Pose2d(x, y, rotation));
}
/**
@@ -94,7 +94,7 @@ public class FieldObject2d implements AutoCloseable {
public synchronized void setTrajectory(Trajectory trajectory) {
m_poses.clear();
for (Trajectory.State state : trajectory.getStates()) {
m_poses.add(state.poseMeters);
m_poses.add(state.pose);
}
updateEntry();
}