Consistently use dimension-specific measure types wherever possible

Allows for more obvious types for readability and access to type-specific methods that would otherwise be unusable (eg `Frequency.asPeriod()`)

Some places must still be generic (eg trapezoid profile constraints and states)

wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/sysid/SysIdRoutineTest.java

@@ -14,8 +14,7 @@
 import static org.mockito.Mockito.verify;
 
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.VoltageUnit;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.simulation.SimHooks;
 import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -28,7 +27,7 @@ class SysIdRoutineTest {
   interface Mechanism extends Subsystem {
     void recordState(SysIdRoutineLog.State state);
 
-    void drive(Measure<VoltageUnit> voltage);
+    void drive(Voltage voltage);
 
     void log(SysIdRoutineLog log);
   }

wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.java

@@ -6,8 +6,7 @@
 
 import static edu.wpi.first.units.Units.Volts;
 
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.VoltageUnit;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.wpilibj.RobotController;
 
 /** Interface for motor controlling devices. */
@@ -45,7 +44,7 @@ default void setVoltage(double outputVolts) {
    *
    * @param outputVoltage The voltage to output.
    */
-  default void setVoltage(Measure<VoltageUnit> outputVoltage) {
+  default void setVoltage(Voltage outputVoltage) {
     setVoltage(outputVoltage.in(Volts));
   }
 

wpilibj/src/main/java/edu/wpi/first/wpilibj/sysid/SysIdRoutineLog.java

@@ -12,15 +12,14 @@
 import static edu.wpi.first.units.Units.Second;
 import static edu.wpi.first.units.Units.Volts;
 
-import edu.wpi.first.units.AngleUnit;
-import edu.wpi.first.units.AngularAccelerationUnit;
-import edu.wpi.first.units.AngularVelocityUnit;
-import edu.wpi.first.units.CurrentUnit;
-import edu.wpi.first.units.DistanceUnit;
-import edu.wpi.first.units.LinearAccelerationUnit;
-import edu.wpi.first.units.LinearVelocityUnit;
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.VoltageUnit;
+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.units.measure.Current;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.LinearAcceleration;
+import edu.wpi.first.units.measure.LinearVelocity;
+import edu.wpi.first.units.measure.Voltage;
 import edu.wpi.first.util.datalog.DoubleLogEntry;
 import edu.wpi.first.util.datalog.StringLogEntry;
 import edu.wpi.first.wpilibj.DataLogManager;
@@ -117,7 +116,7 @@ public MotorLog value(String name, double value, String unit) {
      * @param voltage The voltage to record.
      * @return The motor log (for call chaining).
      */
-    public MotorLog voltage(Measure<VoltageUnit> voltage) {
+    public MotorLog voltage(Voltage voltage) {
       return value("voltage", voltage.in(Volts), Volts.name());
     }
 
@@ -127,7 +126,7 @@ public MotorLog voltage(Measure<VoltageUnit> voltage) {
      * @param position The linear position to record.
      * @return The motor log (for call chaining).
      */
-    public MotorLog linearPosition(Measure<DistanceUnit> position) {
+    public MotorLog linearPosition(Distance position) {
       return value("position", position.in(Meters), Meters.name());
     }
 
@@ -137,7 +136,7 @@ public MotorLog linearPosition(Measure<DistanceUnit> position) {
      * @param position The angular position to record.
      * @return The motor log (for call chaining).
      */
-    public MotorLog angularPosition(Measure<AngleUnit> position) {
+    public MotorLog angularPosition(Angle position) {
       return value("position", position.in(Rotations), Rotations.name());
     }
 
@@ -147,7 +146,7 @@ public MotorLog angularPosition(Measure<AngleUnit> position) {
      * @param velocity The linear velocity to record.
      * @return The motor log (for call chaining).
      */
-    public MotorLog linearVelocity(Measure<LinearVelocityUnit> velocity) {
+    public MotorLog linearVelocity(LinearVelocity velocity) {
       return value("velocity", velocity.in(MetersPerSecond), MetersPerSecond.name());
     }
 
@@ -157,7 +156,7 @@ public MotorLog linearVelocity(Measure<LinearVelocityUnit> velocity) {
      * @param velocity The angular velocity to record.
      * @return The motor log (for call chaining).
      */
-    public MotorLog angularVelocity(Measure<AngularVelocityUnit> velocity) {
+    public MotorLog angularVelocity(AngularVelocity velocity) {
       return value("velocity", velocity.in(RotationsPerSecond), RotationsPerSecond.name());
     }
 
@@ -169,7 +168,7 @@ public MotorLog angularVelocity(Measure<AngularVelocityUnit> velocity) {
      * @param acceleration The linear acceleration to record.
      * @return The motor log (for call chaining).
      */
-    public MotorLog linearAcceleration(Measure<LinearAccelerationUnit> acceleration) {
+    public MotorLog linearAcceleration(LinearAcceleration acceleration) {
       return value(
           "acceleration",
           acceleration.in(MetersPerSecond.per(Second)),
@@ -184,7 +183,7 @@ public MotorLog linearAcceleration(Measure<LinearAccelerationUnit> acceleration)
      * @param acceleration The angular acceleration to record.
      * @return The motor log (for call chaining).
      */
-    public MotorLog angularAcceleration(Measure<AngularAccelerationUnit> acceleration) {
+    public MotorLog angularAcceleration(AngularAcceleration acceleration) {
       return value(
           "acceleration",
           acceleration.in(RotationsPerSecond.per(Second)),
@@ -199,7 +198,7 @@ public MotorLog angularAcceleration(Measure<AngularAccelerationUnit> acceleratio
      * @param current The current to record.
      * @return The motor log (for call chaining).
      */
-    public MotorLog current(Measure<CurrentUnit> current) {
+    public MotorLog current(Current current) {
       value("current", current.in(Amps), Amps.name());
       return this;
     }

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java

@@ -13,8 +13,7 @@
 import edu.wpi.first.math.geometry.proto.Pose2dProto;
 import edu.wpi.first.math.geometry.struct.Pose2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
-import edu.wpi.first.units.DistanceUnit;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Collections;
@@ -76,7 +75,7 @@ public Pose2d(double x, double y, Rotation2d rotation) {
    * @param y The y component of the translational component of the pose.
    * @param rotation The rotational component of the pose.
    */
-  public Pose2d(Measure<DistanceUnit> x, Measure<DistanceUnit> y, Rotation2d rotation) {
+  public Pose2d(Distance x, Distance y, Rotation2d rotation) {
     this(x.in(Meters), y.in(Meters), rotation);
   }
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java

@@ -16,8 +16,6 @@
 import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.units.AngleUnit;
-import edu.wpi.first.units.Measure;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
@@ -131,7 +129,7 @@ public Rotation2d(double x, double y) {
    *
    * @param angle The angle of the rotation.
    */
-  public Rotation2d(Measure<AngleUnit> angle) {
+  public Rotation2d(Angle angle) {
     this(angle.in(Radians));
   }
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java

@@ -8,8 +8,7 @@
 
 import edu.wpi.first.math.geometry.proto.Transform2dProto;
 import edu.wpi.first.math.geometry.struct.Transform2dStruct;
-import edu.wpi.first.units.DistanceUnit;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
@@ -75,7 +74,7 @@ public Transform2d(double x, double y, Rotation2d rotation) {
    * @param y The y component of the translational component of the transform.
    * @param rotation The rotational component of the transform.
    */
-  public Transform2d(Measure<DistanceUnit> x, Measure<DistanceUnit> y, Rotation2d rotation) {
+  public Transform2d(Distance x, Distance y, Rotation2d rotation) {
     this(x.in(Meters), y.in(Meters), rotation);
   }
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java

@@ -17,8 +17,7 @@
 import edu.wpi.first.math.geometry.struct.Translation2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N2;
-import edu.wpi.first.units.DistanceUnit;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Collections;
@@ -84,7 +83,7 @@ public Translation2d(double distance, Rotation2d angle) {
    * @param x The x component of the translation.
    * @param y The y component of the translation.
    */
-  public Translation2d(Measure<DistanceUnit> x, Measure<DistanceUnit> y) {
+  public Translation2d(Distance x, Distance y) {
     this(x.in(Meters), y.in(Meters));
   }
 

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java

@@ -17,8 +17,7 @@
 import edu.wpi.first.math.geometry.struct.Translation3dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.numbers.N3;
-import edu.wpi.first.units.DistanceUnit;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
@@ -89,7 +88,7 @@ public Translation3d(double distance, Rotation3d angle) {
    * @param y The y component of the translation.
    * @param z The z component of the translation.
    */
-  public Translation3d(Measure<DistanceUnit> x, Measure<DistanceUnit> y, Measure<DistanceUnit> z) {
+  public Translation3d(Distance x, Distance y, Distance z) {
     this(x.in(Meters), y.in(Meters), z.in(Meters));
   }
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java

@@ -11,8 +11,7 @@
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto;
 import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct;
-import edu.wpi.first.units.DistanceUnit;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 
@@ -58,7 +57,7 @@ public DifferentialDriveKinematics(double trackWidthMeters) {
    *     between the left wheels and right wheels. However, the empirical value may be larger than
    *     the physical measured value due to scrubbing effects.
    */
-  public DifferentialDriveKinematics(Measure<DistanceUnit> trackWidth) {
+  public DifferentialDriveKinematics(Distance trackWidth) {
     this(trackWidth.in(Meters));
   }
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java

@@ -10,8 +10,7 @@
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelPositionsProto;
 import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct;
-import edu.wpi.first.units.DistanceUnit;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 import java.util.Objects;
 
 /** Represents the wheel positions for a differential drive drivetrain. */
@@ -48,7 +47,7 @@ public DifferentialDriveWheelPositions(double leftMeters, double rightMeters) {
    * @param left Distance measured by the left side.
    * @param right Distance measured by the right side.
    */
-  public DifferentialDriveWheelPositions(Measure<DistanceUnit> left, Measure<DistanceUnit> right) {
+  public DifferentialDriveWheelPositions(Distance left, Distance right) {
     this(left.in(Meters), right.in(Meters));
   }
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java

@@ -11,8 +11,7 @@
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.kinematics.proto.SwerveModulePositionProto;
 import edu.wpi.first.math.kinematics.struct.SwerveModulePositionStruct;
-import edu.wpi.first.units.DistanceUnit;
-import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
@@ -55,7 +54,7 @@ public SwerveModulePosition(double distanceMeters, Rotation2d angle) {
    * @param distance The distance measured by the wheel of the module.
    * @param angle The angle of the module.
    */
-  public SwerveModulePosition(Measure<DistanceUnit> distance, Rotation2d angle) {
+  public SwerveModulePosition(Distance distance, Rotation2d angle) {
     this(distance.in(Meters), angle);
   }
 

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java

@@ -4,13 +4,12 @@
 
 package edu.wpi.first.math.kinematics;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.proto.SwerveModuleStateProto;
 import edu.wpi.first.math.kinematics.struct.SwerveModuleStateStruct;
-import edu.wpi.first.units.DistanceUnit;
-import edu.wpi.first.units.Measure;
-import edu.wpi.first.units.PerUnit;
-import edu.wpi.first.units.TimeUnit;
+import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
@@ -50,9 +49,8 @@ public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
    * @param speed The speed of the wheel of the module.
    * @param angle The angle of the module.
    */
-  public SwerveModuleState(
-      Measure<? extends PerUnit<DistanceUnit, TimeUnit>> speed, Rotation2d angle) {
-    this(speed.baseUnitMagnitude(), angle);
+  public SwerveModuleState(LinearVelocity speed, Rotation2d angle) {
+    this(speed.in(MetersPerSecond), angle);
   }
 
   @Override

wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java

@@ -10,7 +10,6 @@
 import edu.wpi.first.units.PerUnit;
 import edu.wpi.first.units.TimeUnit;
 import edu.wpi.first.units.Unit;
-import edu.wpi.first.units.measure.Velocity;
 import java.util.Objects;
 
 /**
@@ -118,7 +117,8 @@ public State(double position, double velocity) {
      * @param position The position at this state.
      * @param velocity The velocity at this state.
      */
-    public <U extends Unit> State(Measure<U> position, Velocity<U> velocity) {
+    public <U extends Unit> State(
+        Measure<U> position, Measure<? extends PerUnit<? extends U, TimeUnit>> velocity) {
       this(position.baseUnitMagnitude(), velocity.baseUnitMagnitude());
     }