[wpiunits] Java units API rewrite (#6958)

Java generics are too limited to do what we need. This refactors generic code previously in Unit and Measure into unit-specific classes that can have unit-safe math operations (notably, times and divide) that can return values in known units instead of a wildcarded Measure<?>.

Unit-specific measure implementations are automatically generated by ./wpiunits/generate_units.py, which generates generic interfaces and mutable and immutable implementations of those interfaces. These make up the bulk of the diff of this PR (approximately 9300 LOC).

This also adds units for angular and linear velocities, accelerations, and momenta; moment of inertia; and torque.
This commit is contained in:
Sam Carlberg
2024-09-07 13:59:29 -04:00
committed by GitHub
parent 496e7c1bba
commit a9b885070e
178 changed files with 14750 additions and 2158 deletions

View File

@@ -7,8 +7,7 @@ package edu.wpi.first.wpilibj.examples.addressableled;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.wpilibj.AddressableLED;
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
import edu.wpi.first.wpilibj.LEDPattern;
@@ -23,7 +22,7 @@ public class Robot extends TimedRobot {
private final LEDPattern m_rainbow = LEDPattern.rainbow(255, 128);
// Our LED strip has a density of 120 LEDs per meter
private static final Measure<Distance> kLedSpacing = Meters.of(1 / 120.0);
private static final Distance kLedSpacing = Meters.of(1 / 120.0);
// Create a new pattern that scrolls the rainbow pattern across the LED strip, moving at a speed
// of 1 meter per second.

View File

@@ -4,16 +4,13 @@
package edu.wpi.first.wpilibj.examples.sysid.subsystems;
import static edu.wpi.first.units.MutableMeasure.mutable;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.units.measure.MutDistance;
import edu.wpi.first.units.measure.MutLinearVelocity;
import edu.wpi.first.units.measure.MutVoltage;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -50,11 +47,11 @@ public class Drive extends SubsystemBase {
DriveConstants.kRightEncoderReversed);
// Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
private final MutVoltage m_appliedVoltage = Volts.mutable(0);
// Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
private final MutableMeasure<Distance> m_distance = mutable(Meters.of(0));
private final MutDistance m_distance = Meters.mutable(0);
// Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
private final MutableMeasure<Velocity<Distance>> m_velocity = mutable(MetersPerSecond.of(0));
private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0);
// Create a new SysId routine for characterizing the drive.
private final SysIdRoutine m_sysIdRoutine =
@@ -63,9 +60,9 @@ public class Drive extends SubsystemBase {
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
// Tell SysId how to plumb the driving voltage to the motors.
(Measure<Voltage> volts) -> {
m_leftMotor.setVoltage(volts.in(Volts));
m_rightMotor.setVoltage(volts.in(Volts));
voltage -> {
m_leftMotor.setVoltage(voltage);
m_rightMotor.setVoltage(voltage);
},
// Tell SysId how to record a frame of data for each motor on the mechanism being
// characterized.

View File

@@ -4,18 +4,17 @@
package edu.wpi.first.wpilibj.examples.sysid.subsystems;
import static edu.wpi.first.units.MutableMeasure.mutable;
import static edu.wpi.first.units.Units.Radians;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Rotations;
import static edu.wpi.first.units.Units.RotationsPerSecond;
import static edu.wpi.first.units.Units.Volts;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.units.Angle;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.units.measure.MutAngle;
import edu.wpi.first.units.measure.MutAngularVelocity;
import edu.wpi.first.units.measure.MutVoltage;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.examples.sysid.Constants.ShooterConstants;
@@ -40,11 +39,11 @@ public class Shooter extends SubsystemBase {
ShooterConstants.kEncoderReversed);
// Mutable holder for unit-safe voltage values, persisted to avoid reallocation.
private final MutableMeasure<Voltage> m_appliedVoltage = mutable(Volts.of(0));
private final MutVoltage m_appliedVoltage = Volts.mutable(0);
// Mutable holder for unit-safe linear distance values, persisted to avoid reallocation.
private final MutableMeasure<Angle> m_angle = mutable(Rotations.of(0));
private final MutAngle m_angle = Radians.mutable(0);
// Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation.
private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RotationsPerSecond.of(0));
private final MutAngularVelocity m_velocity = RadiansPerSecond.mutable(0);
// Create a new SysId routine for characterizing the shooter.
private final SysIdRoutine m_sysIdRoutine =
@@ -53,9 +52,7 @@ public class Shooter extends SubsystemBase {
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
// Tell SysId how to plumb the driving voltage to the motor(s).
(Measure<Voltage> volts) -> {
m_shooterMotor.setVoltage(volts.in(Volts));
},
m_shooterMotor::setVoltage,
// Tell SysId how to record a frame of data for each motor on the mechanism being
// characterized.
log -> {