Changed SysID methods to allow logging in custom units

wpilibc/src/main/native/include/frc/sysid/SysIdRoutineLog.h

@@ -61,87 +61,127 @@ class SysIdRoutineLog {
     /**
      * Log the voltage applied to the motor.
      *
+     * The value is recorded in volts by default;
+     * to change this, specify your target unit as a generic bound.
+     *
      * @param voltage The voltage to record.
      * @return The motor log (for call chaining).
      */
+    template <typename U = units::volt_t>
     MotorLog& voltage(units::volt_t voltage) {
-      return value("voltage", voltage.value(), voltage.name());
+      U converted{voltage};
+      return value("voltage", converted.value(), converted.name());
     }
 
     /**
      * Log the linear position of the motor.
      *
+     * The value is recorded in meters by default;
+     * to change this, specify your target unit as a generic bound.
+     *
      * @param position The linear position to record.
      * @return The motor log (for call chaining).
      */
+    template <typename U = units::meter_t>
     MotorLog& position(units::meter_t position) {
-      return value("position", position.value(), position.name());
+      U converted{position};
+      return value("position", converted.value(), converted.name());
     }
 
     /**
      * Log the angular position of the motor.
      *
+     * The value is recorded in rotations by default;
+     * to change this, specify your target unit as a generic bound.
+     *
      * @param position The angular position to record.
      * @return The motor log (for call chaining).
      */
+    template <typename U = units::turn_t>
     MotorLog& position(units::turn_t position) {
-      return value("position", position.value(), position.name());
+      U converted{position};
+      return value("position", converted.value(), converted.name());
     }
 
     /**
      * Log the linear velocity of the motor.
      *
+     * The value is recorded in meters/second by default;
+     * to change this, specify your target unit as a generic bound.
+     *
      * @param velocity The linear velocity to record.
      * @return The motor log (for call chaining).
      */
+    template <typename U = units::meters_per_second_t>
     MotorLog& velocity(units::meters_per_second_t velocity) {
-      return value("velocity", velocity.value(), velocity.name());
+      U converted{velocity};
+      return value("velocity", converted.value(), converted.name());
     }
 
     /**
      * Log the angular velocity of the motor.
      *
+     * The value is recorded in rotations/second by default;
+     * to change this, specify your target unit as a generic bound.
+     *
      * @param velocity The angular velocity to record.
      * @return The motor log (for call chaining).
      */
+    template <typename U = units::turns_per_second_t>
     MotorLog& velocity(units::turns_per_second_t velocity) {
-      return value("velocity", velocity.value(), velocity.name());
+      U converted{velocity};
+      return value("velocity", converted.value(), converted.name());
     }
 
     /**
      * Log the linear acceleration of the motor.
      *
      * This is optional; SysId can perform an accurate fit without it.
      *
+     * The value is recorded in meters/second^2 by default;
+     * to change this, specify your target unit as a generic bound.
+     *
      * @param acceleration The linear acceleration to record.
      * @return The motor log (for call chaining).
      */
+    template <typename U = units::meters_per_second_squared_t>
     MotorLog& acceleration(units::meters_per_second_squared_t acceleration) {
-      return value("acceleration", acceleration.value(), acceleration.name());
+      U converted{acceleration};
+      return value("acceleration", converted.value(), converted.name());
     }
 
     /**
      * Log the angular acceleration of the motor.
      *
      * This is optional; SysId can perform an accurate fit without it.
      *
+     * The value is recorded in rotations/second^2 by default;
+     * to change this, specify your target unit as a generic bound.
+     *
      * @param acceleration The angular acceleration to record.
      * @return The motor log (for call chaining).
      */
+    template <typename U = units::turns_per_second_squared_t>
     MotorLog& acceleration(units::turns_per_second_squared_t acceleration) {
-      return value("acceleration", acceleration.value(), acceleration.name());
+      U converted{acceleration};
+      return value("acceleration", converted.value(), converted.name());
     }
 
     /**
      * Log the current applied to the motor.
      *
      * This is optional; SysId can perform an accurate fit without it.
      *
+     * The value is recorded in amperes by default;
+     * to change this, specify your target unit as a generic bound.
+     *
      * @param current The current to record.
      * @return The motor log (for call chaining).
      */
+    template <typename U = units::ampere_t>
     MotorLog& current(units::ampere_t current) {
-      return value("current", current.value(), current.name());
+      U converted{current};
+      return value("current", converted.value(), converted.name());
     }
 
    private:

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

@@ -7,11 +7,20 @@
 import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.Meters;
 import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
 import static edu.wpi.first.units.Units.Rotations;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
-import static edu.wpi.first.units.Units.Second;
+import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond;
 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.VoltageUnit;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularAcceleration;
 import edu.wpi.first.units.measure.AngularVelocity;
@@ -111,96 +120,185 @@ public MotorLog value(String name, double value, String unit) {
     }
 
     /**
-     * Log the voltage applied to the motor.
+     * Log the voltage applied to the motor(in volts).
      *
      * @param voltage The voltage to record.
      * @return The motor log (for call chaining).
      */
     public MotorLog voltage(Voltage voltage) {
-      return value("voltage", voltage.in(Volts), Volts.name());
+      return voltage(voltage, Volts);
     }
 
     /**
-     * Log the linear position of the motor.
+     * Log the voltage applied to the motor.
+     *
+     * @param voltage The voltage to record.
+     * @param unit The unit to log the voltage in.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog voltage(Voltage voltage, VoltageUnit unit) {
+      return value("voltage", voltage.in(unit), unit.name());
+    }
+
+    /**
+     * Log the linear position of the motor(in meters).
      *
      * @param position The linear position to record.
      * @return The motor log (for call chaining).
      */
     public MotorLog linearPosition(Distance position) {
-      return value("position", position.in(Meters), Meters.name());
+      return linearPosition(position, Meters);
     }
 
     /**
-     * Log the angular position of the motor.
+     * Log the linear position of the motor.
+     *
+     * @param position The linear position to record.
+     * @param unit The unit to log the position in.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog linearPosition(Distance position, DistanceUnit unit) {
+      return value("position", position.in(unit), unit.name());
+    }
+
+    /**
+     * Log the angular position of the motor(in rotations).
      *
      * @param position The angular position to record.
      * @return The motor log (for call chaining).
      */
     public MotorLog angularPosition(Angle position) {
-      return value("position", position.in(Rotations), Rotations.name());
+      return angularPosition(position, Rotations);
     }
 
     /**
-     * Log the linear velocity of the motor.
+     * Log the angular position of the motor.
+     *
+     * @param position The angular position to record.
+     * @param unit The unit to log the position in.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog angularPosition(Angle position, AngleUnit unit) {
+      return value("position", position.in(unit), unit.name());
+    }
+
+    /**
+     * Log the linear velocity of the motor(in meters per second).
      *
      * @param velocity The linear velocity to record.
      * @return The motor log (for call chaining).
      */
     public MotorLog linearVelocity(LinearVelocity velocity) {
-      return value("velocity", velocity.in(MetersPerSecond), MetersPerSecond.name());
+      return linearVelocity(velocity, MetersPerSecond);
     }
 
     /**
-     * Log the angular velocity of the motor.
+     * Log the linear velocity of the motor.
+     *
+     * @param velocity The linear velocity to record.
+     * @param unit The unit to log the velocity in.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog linearVelocity(LinearVelocity velocity, LinearVelocityUnit unit) {
+      return value("velocity", velocity.in(unit), unit.name());
+    }
+
+    /**
+     * Log the angular velocity of the motor(in rotations per second).
      *
      * @param velocity The angular velocity to record.
      * @return The motor log (for call chaining).
      */
     public MotorLog angularVelocity(AngularVelocity velocity) {
-      return value("velocity", velocity.in(RotationsPerSecond), RotationsPerSecond.name());
+      return angularVelocity(velocity, RotationsPerSecond);
     }
 
     /**
-     * Log the linear acceleration of the motor.
+     * Log the angular velocity of the motor.
+     *
+     * @param velocity The angular velocity to record.
+     * @param unit The unit to log the velocity in.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog angularVelocity(AngularVelocity velocity, AngularVelocityUnit unit) {
+      return value("velocity", velocity.in(unit), unit.name());
+    }
+
+    /**
+     * Log the linear acceleration of the motor(in meters per second squared).
      *
      * <p>This is optional; SysId can perform an accurate fit without it.
      *
      * @param acceleration The linear acceleration to record.
      * @return The motor log (for call chaining).
      */
     public MotorLog linearAcceleration(LinearAcceleration acceleration) {
-      return value(
-          "acceleration",
-          acceleration.in(MetersPerSecond.per(Second)),
-          MetersPerSecond.per(Second).name());
+      return linearAcceleration(acceleration, MetersPerSecondPerSecond);
     }
 
     /**
-     * Log the angular acceleration of the motor.
+     * Log the linear acceleration of the motor.
+     *
+     * <p>This is optional; SysId can perform an accurate fit without it.
+     *
+     * @param acceleration The linear acceleration to record.
+     * @param unit The unit to log the acceleration in.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog linearAcceleration(
+        LinearAcceleration acceleration, LinearAccelerationUnit unit) {
+      return value("acceleration", acceleration.in(unit), unit.name());
+    }
+
+    /**
+     * Log the angular acceleration of the motor(in rotations per second squared).
      *
      * <p>This is optional; SysId can perform an accurate fit without it.
      *
      * @param acceleration The angular acceleration to record.
      * @return The motor log (for call chaining).
      */
     public MotorLog angularAcceleration(AngularAcceleration acceleration) {
-      return value(
-          "acceleration",
-          acceleration.in(RotationsPerSecond.per(Second)),
-          RotationsPerSecond.per(Second).name());
+      return angularAcceleration(acceleration, RotationsPerSecondPerSecond);
     }
 
     /**
-     * Log the current applied to the motor.
+     * Log the angular acceleration of the motor.
+     *
+     * <p>This is optional; SysId can perform an accurate fit without it.
+     *
+     * @param acceleration The angular acceleration to record.
+     * @param unit The unit to log the acceleration in.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog angularAcceleration(
+        AngularAcceleration acceleration, AngularAccelerationUnit unit) {
+      return value("acceleration", acceleration.in(unit), unit.name());
+    }
+
+    /**
+     * Log the current applied to the motor(in amps).
      *
      * <p>This is optional; SysId can perform an accurate fit without it.
      *
      * @param current The current to record.
      * @return The motor log (for call chaining).
      */
     public MotorLog current(Current current) {
-      value("current", current.in(Amps), Amps.name());
-      return this;
+      return current(current, Amps);
+    }
+
+    /**
+     * Log the current applied to the motor.
+     *
+     * <p>This is optional; SysId can perform an accurate fit without it.
+     *
+     * @param current The current to record.
+     * @param unit The unit to log the current in.
+     * @return The motor log (for call chaining).
+     */
+    public MotorLog current(Current current, CurrentUnit unit) {
+      return value("current", current.in(unit), unit.name());
     }
   }