mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-01 02:41:48 +00:00
[wpiunits] Add subproject for a Java typesafe unit system (#5371)
# Background
Unit safety has always been a problem in WPILib. Any value corresponding to a physical measurement, such as current draw or distance traveled, is represented by a bare number with no unit tied to it; it's up to the programmer to know what units they're working and take care to remember that while working on their robot program. This leads to bugs when programmers accidentally mix units without knowing, or measure something (such as a wheel diameter) in one unit and program using another. `wpiunits` is intended to eliminate that class of bugs.
Another source of friction is the controllers and models in `wpimath` that expect all inputs to be in terms of SI units (meter, kilogram, and so on), while most FRC teams are US-based and most commonly use imperial units. wpimath does a good job of noting unit types in method names and argument names; however, it still relies on users properly converting values (and knowing they even have to do so).
# API
There are really only two core classes in this library: `Unit` and `Measure`. A `Unit` represents some dimension like distance or time. `Unit` is subclassed to define specific dimensions (eg `Distance` and `Time`) and those subclasses are instantiated to defined particular units in those dimensions, such as `Meters` and `Feet` being instances of the `Distance` class.
A `Measure` is a value tied to a particular dimension like distance and knows what unit that value is tied to. `Measure` has two implementations - one immutable and one mutable. The `Measure` interface only defines *read-only* operations; any API working with measurements should use the interface. The default implementation is `ImmutableMeasure`, which only implements those read-only operations and is useful for tracking constants. `MutableMeasure` also adds some methods that will allow for mutation of its internal state; this class is intended for use for things like sensors and controllers that track internal state and don't want to allocate new `Measure` objects every time something like `myEncoder.getDistance()` is called. However, the APIs for those methods should still only expose the read-only `Measure` interface so users can't (without casting or reflection) change the internal values.
A `Units` class provides convenient definitions for most of the commonly used unit types, such as `Meters`, `Feet`, and `Milliseconds`. I recommend static importing these units eg `import static edu.wpi.first.units.Units.Meters`) so they can be used like `Meters.of(1.234)` instead of `Units.Meters.of(1.234)`
# Examples
These examples are admittedly contrived. Users shouldn't be interacting much with measure objects themselves, since wpimath and wpilibj classes will be updated to support working with them; users will often just have to take a `Measure` output from one place (such as an encoder) and feed it as input to something else (such as a PID controller or kinematics model)
```java
// Using raw units
Encoder encoder = ...
int kPulsesPerRev = 2048;
double kWheelDiameterMeters = Units.inchesToMeters(6);
double kGearRatio = 10.86;
// always have to remember this encoder will output in meters!
encoder.setDistancePerPulse(kWheelDiameterMeters * Math.PI / (kGearRatio * kPulsesPerRev));
Command driveDistance(double distance) {
// have to know the distance argument needs to be in meters!
return run(this::driveStraight).until(() -> encoder.getDistance() >= distance);
}
// Oops! This will go 16 feet, not 5!
Command driveFiveFeet = driveDistance(5);
Command driveOneMeter = driveDistance(1);
```
```java
// Using wpiunits
Encoder encoder = ...
int kPulsesPerRev = 2048;
Measure<Distance> kWheelDiameter = Inches.of(6);
double kGearRatio = 10.86;
encoder.setDistancePerPulse(kWheelDiameter.times(Math.PI).divide(kGearRatio * kPulsesPerRev));
Command driveDistance(Measure<Distance> distance) {
// Measure#gte automatically handles unit conversions
return run(this::driveStraight).until(() -> encoder.getDistance().gte(distance));
}
// Users HAVE to be explicit about their units
Command driveFiveFeet = driveDistance(Feet.of(5));
Command driveOneMeter = driveDistance(Meters.of(1));
```
```java
SmartDashboard.putNumber("Temperature (C)", pdp.getTemperature().in(Celsius));
SmartDashboard.putNumber("Temperature (F)", pdp.getTemperature().in(Fahrenheit));
```
```java
var InchSecond = Inch.mult(Second); // new combined unit types can be user-defined
var InchPerSecond = Inch.per(Second);
PIDController<Distance, ElectricPotential> heightController = new PIDController<>(
/* kP */ Volts.of(0.2).per(Inch),
/* kI */ Volts.of(0.002).per(InchSecond),
/* kD */ Volts.of(0.008).per(InchPerSecond)
);
var elevatorTop = Feet.of(4).plus(Inches.of(6.125));
elevatorMotor.setVoltage(heightController.calculate(encoder.getDistance(), elevatorTop));
```
This commit is contained in:
Sam Carlberg
@@ -0,0 +1,332 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.units.collections;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Arrays;
|
||||
import java.util.Collection;
|
||||
import java.util.List;
|
||||
|
||||
/**
|
||||
* A variant on {@code java.util.HashMap<K, V>} that uses primitive long ints for map keys instead
|
||||
* of autoboxed Long objects like would be used for a {@code Map<Long, V>}.
|
||||
*
|
||||
* @param <V> the type of the values stored in the map
|
||||
*/
|
||||
public class LongToObjectHashMap<V> {
|
||||
private static final int kInitialSize = 0;
|
||||
private static final int kInitialCapacity = 8; // NOTE: must be a power of two
|
||||
|
||||
/**
|
||||
* The default load factor of the hashmap. If the ratio of the number of entries to the map's
|
||||
* capacity exceeds this value, the map will be resized (doubled capacity) in order for more
|
||||
* values to be easily inserted.
|
||||
*/
|
||||
private static final double kLoadFactor = 75.00 / 100;
|
||||
|
||||
/** The current number of key-value pairs in the map. */
|
||||
private int m_size = kInitialSize;
|
||||
|
||||
/**
|
||||
* The current maximum capacity of the map. Note that it will be resized before m_size reaches
|
||||
* this value.
|
||||
*/
|
||||
private int m_capacity = kInitialCapacity;
|
||||
|
||||
/**
|
||||
* The keys in the map. This is a sparse array, and the location of a key may not be equal to the
|
||||
* result of calling {@link #bucket(long)} on that key. To handle hash collisions, if a bucket is
|
||||
* already in use when trying to insert a value, the bucket number is incremented (wrapping around
|
||||
* to 0 if it's equal to m_capacity) and <i>that</i> bucket is checked to see if it's available.
|
||||
* This process continues until an empty bucket is found (which is guaranteed because m_size is
|
||||
* always less than m_capacity).
|
||||
*/
|
||||
private long[] m_keys = new long[m_capacity];
|
||||
|
||||
/** Tracks which buckets are actually used (have a key-value mapping). */
|
||||
private boolean[] m_uses = new boolean[m_capacity];
|
||||
|
||||
/**
|
||||
* The values in the map. See the documentation for m_keys for how indexing into this array works.
|
||||
*/
|
||||
@SuppressWarnings("unchecked")
|
||||
private V[] m_values = (V[]) new Object[m_capacity];
|
||||
|
||||
/**
|
||||
* Puts a value {@code value} corresponding to key {@code key} in the map.
|
||||
*
|
||||
* @param key the associated key
|
||||
* @param value the value to insert
|
||||
* @return the previous value that was mapped to the key, or null if no such value existed
|
||||
*/
|
||||
public V put(long key, V value) {
|
||||
int bucket = bucket(key);
|
||||
|
||||
// Increment the bucket until we hit an open space (there's always going to be at least one)
|
||||
while (m_uses[bucket]) {
|
||||
if (m_keys[bucket] == key) {
|
||||
// replace the existing value
|
||||
var oldValue = m_values[bucket];
|
||||
m_values[bucket] = value;
|
||||
return oldValue;
|
||||
}
|
||||
bucket = safeIncrement(bucket);
|
||||
}
|
||||
|
||||
m_uses[bucket] = true;
|
||||
m_keys[bucket] = key;
|
||||
m_values[bucket] = value;
|
||||
m_size++;
|
||||
|
||||
if (m_size > maxSize()) {
|
||||
grow();
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the value associated with the given key.
|
||||
*
|
||||
* @param key the key to retrieve the value for
|
||||
* @return the value mapped to the key, or null if the key is not in the map
|
||||
*/
|
||||
public V get(long key) {
|
||||
int bucket = bucket(key);
|
||||
while (m_uses[bucket]) {
|
||||
if (m_keys[bucket] == key) {
|
||||
// found it
|
||||
return m_values[bucket];
|
||||
}
|
||||
bucket = safeIncrement(bucket);
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
/**
|
||||
* Removes the value associated with the given key and returns it.
|
||||
*
|
||||
* @param key the key to remove
|
||||
* @return the value corresponding to the key, or null if the key is not in the map
|
||||
*/
|
||||
public V remove(long key) {
|
||||
int bucket = bucket(key);
|
||||
while (m_uses[bucket]) {
|
||||
if (m_keys[bucket] == key) {
|
||||
// found it
|
||||
// TODO: Shrink the map when below a certain load factor
|
||||
// Current use cases don't remove elements from the map, so there's not much use
|
||||
// for shrinking at the moment.
|
||||
m_size--;
|
||||
m_keys[bucket] = 0L;
|
||||
m_uses[bucket] = false;
|
||||
|
||||
var oldValue = m_values[bucket];
|
||||
m_values[bucket] = null;
|
||||
return oldValue;
|
||||
}
|
||||
bucket = safeIncrement(bucket);
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if a key is contained in the map.
|
||||
*
|
||||
* @param key the key to check
|
||||
* @return true if the key has an associated value, false if not
|
||||
*/
|
||||
public boolean containsKey(long key) {
|
||||
int bucket = bucket(key);
|
||||
while (m_uses[bucket]) {
|
||||
if (m_keys[bucket] == key) {
|
||||
return true;
|
||||
}
|
||||
bucket = safeIncrement(bucket);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/** Clears and removes all entries from the map. */
|
||||
public void clear() {
|
||||
if (m_size == 0) {
|
||||
// Nothing to do
|
||||
return;
|
||||
}
|
||||
m_size = 0;
|
||||
|
||||
Arrays.fill(m_uses, false);
|
||||
Arrays.fill(m_keys, 0L);
|
||||
Arrays.fill(m_values, null);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the number of key-value pairs currently contained in the map.
|
||||
*
|
||||
* @return the current size of the map
|
||||
*/
|
||||
public int size() {
|
||||
return m_size;
|
||||
}
|
||||
|
||||
// package-private for tests
|
||||
int capacity() {
|
||||
return m_capacity;
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the map contains any entries.
|
||||
*
|
||||
* @return true if at least one entry is present, false otherwise
|
||||
*/
|
||||
public boolean isEmpty() {
|
||||
return m_size == 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the keys contained in the map. Ordering is not guaranteed. The returned set is read-only
|
||||
* and immutable. This uses a custom class for primitive long values to avoid unnecessary
|
||||
* autoboxing to {@code java.lang.Long}.
|
||||
*
|
||||
* @return a read-only set of keys
|
||||
*/
|
||||
public ReadOnlyPrimitiveLongSet keySet() {
|
||||
// copy the sparse key array into a compact array
|
||||
final long[] keys = new long[m_size];
|
||||
int i = 0;
|
||||
for (int bucket = 0; bucket < m_capacity; bucket++) {
|
||||
if (m_uses[bucket]) {
|
||||
keys[i] = m_keys[bucket];
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
return new ReadOnlyPrimitiveLongSet(keys);
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the values contained in the map. Ordering is not guaranteed. The returned collection is
|
||||
* read-only and immutable.
|
||||
*
|
||||
* @return a read-only collection of values
|
||||
*/
|
||||
public Collection<V> values() {
|
||||
Collection<V> values = new ArrayList<>();
|
||||
for (int bucket = 0; bucket < m_capacity; bucket++) {
|
||||
if (m_uses[bucket]) {
|
||||
values.add(m_values[bucket]);
|
||||
}
|
||||
}
|
||||
return List.copyOf(values); // return a readonly copy
|
||||
}
|
||||
|
||||
@FunctionalInterface
|
||||
public interface IteratorFunction<V> {
|
||||
void accept(long key, V value);
|
||||
}
|
||||
|
||||
/**
|
||||
* Iterates over every key-value pair in the map and passes them to the given function.
|
||||
*
|
||||
* @param function the function to apply to every key-value pair.
|
||||
*/
|
||||
public void forEach(IteratorFunction<? super V> function) {
|
||||
for (int bucket = 0; bucket < m_capacity; bucket++) {
|
||||
if (m_uses[bucket]) {
|
||||
function.accept(m_keys[bucket], m_values[bucket]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void grow() {
|
||||
final int currentSize = m_size;
|
||||
final int oldCapacity = m_capacity;
|
||||
if (oldCapacity * kLoadFactor >= currentSize) {
|
||||
// We're below the maximum allowed size for the current capacity
|
||||
// Nothing to do
|
||||
return;
|
||||
}
|
||||
|
||||
final int newCapacity = oldCapacity * 2;
|
||||
final int newMask = newCapacity - 1;
|
||||
|
||||
final boolean[] oldUses = m_uses;
|
||||
final long[] oldKeys = m_keys;
|
||||
final V[] oldValues = m_values;
|
||||
|
||||
final boolean[] newUses = new boolean[newCapacity];
|
||||
final long[] newKeys = new long[newCapacity];
|
||||
@SuppressWarnings("unchecked")
|
||||
final V[] newValues = (V[]) new Object[newCapacity];
|
||||
|
||||
for (int oldBucket = 0; oldBucket < oldCapacity; oldBucket++) {
|
||||
if (!oldUses[oldBucket]) {
|
||||
// Bucket is empty, skip
|
||||
continue;
|
||||
}
|
||||
final long key = oldKeys[oldBucket];
|
||||
final V value = oldValues[oldBucket];
|
||||
|
||||
int newBucket = (int) (hash(key) & newMask);
|
||||
while (newUses[newBucket]) {
|
||||
newBucket = (newBucket + 1) & newMask;
|
||||
}
|
||||
|
||||
newUses[newBucket] = true;
|
||||
newKeys[newBucket] = key;
|
||||
newValues[newBucket] = value;
|
||||
}
|
||||
|
||||
m_capacity = newCapacity;
|
||||
m_uses = newUses;
|
||||
m_keys = newKeys;
|
||||
m_values = newValues;
|
||||
}
|
||||
|
||||
private int maxSize() {
|
||||
return (int) (m_capacity * kLoadFactor);
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates a hashcode for an input key. Does some bit shuffling to account for poor hash
|
||||
* functions.
|
||||
*
|
||||
* @param key the key to hash
|
||||
* @return a hashcode for the input key
|
||||
*/
|
||||
private long hash(long key) {
|
||||
return 31 + (key ^ (key >>> 15) ^ (key >>> 31) ^ (key << 31));
|
||||
}
|
||||
|
||||
/**
|
||||
* The mask to use when translating a hashcode to a bucket index. Relies on m_capacity being a
|
||||
* power of two.
|
||||
*/
|
||||
private int mask() {
|
||||
return m_capacity - 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculates the desired bucket index for a particular key. Does nothing to handle the case where
|
||||
* the calculated index is already in use by another key.
|
||||
*
|
||||
* @param key the key to get the bucket for
|
||||
* @return the desired bucket index
|
||||
*/
|
||||
private int bucket(long key) {
|
||||
var hash = hash(key);
|
||||
return (int) (hash & mask());
|
||||
}
|
||||
|
||||
/**
|
||||
* Increments a bucket index by 1, wrapping around to 0 if the index is already at the maximum.
|
||||
*
|
||||
* @param bucket the index to increment
|
||||
* @return the incremented bucket index
|
||||
*/
|
||||
private int safeIncrement(int bucket) {
|
||||
return (bucket + 1) & mask();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,125 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
package edu.wpi.first.units.collections;
|
||||
|
||||
import java.util.Arrays;
|
||||
import java.util.Iterator;
|
||||
import java.util.NoSuchElementException;
|
||||
import java.util.stream.LongStream;
|
||||
|
||||
/** A read-only set of unique primitive {@code long} values. */
|
||||
public class ReadOnlyPrimitiveLongSet implements Iterable<Long> {
|
||||
private final long[] m_values;
|
||||
|
||||
/**
|
||||
* Creates a new set from the given values. These values do not have to be unique.
|
||||
*
|
||||
* @param values the values that belong to the set.
|
||||
*/
|
||||
@SuppressWarnings({"PMD.ForLoopCanBeForeach", "ForLoopReplaceableByForEach"})
|
||||
public ReadOnlyPrimitiveLongSet(long... values) {
|
||||
// initial size is the upper limit
|
||||
long[] uniqueValues = new long[values.length];
|
||||
int numUniqueValues = 0;
|
||||
boolean seenZero = false;
|
||||
|
||||
// copy the set of unique values to our array
|
||||
// using indexed for-loops to avoid allocations
|
||||
copyLoop:
|
||||
for (int i = 0; i < values.length; i++) {
|
||||
long value = values[i];
|
||||
if (value == 0 && !seenZero) {
|
||||
// special case to support zero
|
||||
seenZero = true;
|
||||
} else {
|
||||
for (int j = 0; j < uniqueValues.length; j++) {
|
||||
long uniqueValue = uniqueValues[j];
|
||||
if (uniqueValue == value) {
|
||||
continue copyLoop;
|
||||
}
|
||||
}
|
||||
}
|
||||
uniqueValues[numUniqueValues] = value;
|
||||
numUniqueValues++;
|
||||
}
|
||||
|
||||
if (numUniqueValues == values.length) {
|
||||
// all input values were unique, no need to truncate
|
||||
m_values = uniqueValues;
|
||||
} else {
|
||||
// truncate the array to remove trailing empty space
|
||||
m_values = Arrays.copyOf(uniqueValues, numUniqueValues);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the set contains a particular value.
|
||||
*
|
||||
* @param value the value to check for
|
||||
* @return true if the value is in the set, false if not
|
||||
*/
|
||||
public boolean contains(long value) {
|
||||
for (long mValue : m_values) {
|
||||
if (mValue == value) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Retrieves the number of elements in the set.
|
||||
*
|
||||
* @return the number of elements in the set
|
||||
*/
|
||||
public int size() {
|
||||
return m_values.length;
|
||||
}
|
||||
|
||||
/**
|
||||
* Checks if the set is empty, i.e. contains no values.
|
||||
*
|
||||
* @return true if there are no values in the set, false otherwise.
|
||||
*/
|
||||
public boolean isEmpty() {
|
||||
return size() == 0;
|
||||
}
|
||||
|
||||
/** Creates a stream of primitive long values for the set. */
|
||||
public LongStream stream() {
|
||||
return Arrays.stream(m_values);
|
||||
}
|
||||
|
||||
/**
|
||||
* Creates a new array that contains all of the values in the set.
|
||||
*
|
||||
* @return an array containing all the values in the set
|
||||
*/
|
||||
public long[] toArray() {
|
||||
return Arrays.copyOf(m_values, m_values.length);
|
||||
}
|
||||
|
||||
@Override
|
||||
public Iterator<Long> iterator() {
|
||||
return new Iterator<>() {
|
||||
@SuppressWarnings("PMD.RedundantFieldInitializer")
|
||||
private int m_index = 0;
|
||||
|
||||
@Override
|
||||
public boolean hasNext() {
|
||||
return m_index < ReadOnlyPrimitiveLongSet.this.size();
|
||||
}
|
||||
|
||||
@Override
|
||||
public Long next() {
|
||||
if (!hasNext()) {
|
||||
throw new NoSuchElementException();
|
||||
}
|
||||
|
||||
return ReadOnlyPrimitiveLongSet.this.m_values[m_index++];
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user