[wpimath] Add arithmetic functions to wheel speeds classes (#5465)

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

@@ -154,7 +154,7 @@ public class ChassisSpeeds {
   }
 
   /**
-   * Subtracts the other ChassisSpeeds from the other ChassisSpeeds and returns the difference.
+   * Subtracts the other ChassisSpeeds from the current ChassisSpeeds and returns the difference.
    *
    * <p>For example, ChassisSpeeds{5.0, 4.0, 2.0} - ChassisSpeeds{1.0, 2.0, 1.0} =
    * ChassisSpeeds{4.0, 2.0, 1.0}
@@ -197,8 +197,8 @@ public class ChassisSpeeds {
    *
    * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} / 2 = ChassisSpeeds{1.0, 1.25, 0.5}
    *
-   * @param scalar The scalar to multiply by.
-   * @return The reference to the new mutated object.
+   * @param scalar The scalar to divide by.
+   * @return The scaled ChassisSpeeds.
    */
   public ChassisSpeeds div(double scalar) {
     return new ChassisSpeeds(

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

@@ -46,6 +46,77 @@ public class DifferentialDriveWheelSpeeds {
     }
   }
 
+  /**
+   * Adds two DifferentialDriveWheelSpeeds and returns the sum.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{1.0, 0.5} + DifferentialDriveWheelSpeeds{2.0, 1.5}
+   * = DifferentialDriveWheelSpeeds{3.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to add.
+   * @return The sum of the DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds plus(DifferentialDriveWheelSpeeds other) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond + other.leftMetersPerSecond,
+        rightMetersPerSecond + other.rightMetersPerSecond);
+  }
+
+  /**
+   * Subtracts the other DifferentialDriveWheelSpeeds from the current DifferentialDriveWheelSpeeds
+   * and returns the difference.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{5.0, 4.0} - DifferentialDriveWheelSpeeds{1.0, 2.0}
+   * = DifferentialDriveWheelSpeeds{4.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelSpeeds to subtract.
+   * @return The difference between the two DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds minus(DifferentialDriveWheelSpeeds other) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond - other.leftMetersPerSecond,
+        rightMetersPerSecond - other.rightMetersPerSecond);
+  }
+
+  /**
+   * Returns the inverse of the current DifferentialDriveWheelSpeeds. This is equivalent to negating
+   * all components of the DifferentialDriveWheelSpeeds.
+   *
+   * @return The inverse of the current DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds unaryMinus() {
+    return new DifferentialDriveWheelSpeeds(-leftMetersPerSecond, -rightMetersPerSecond);
+  }
+
+  /**
+   * Multiplies the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} * 2 = DifferentialDriveWheelSpeeds{4.0,
+   * 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds times(double scalar) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond * scalar, rightMetersPerSecond * scalar);
+  }
+
+  /**
+   * Divides the DifferentialDriveWheelSpeeds by a scalar and returns the new
+   * DifferentialDriveWheelSpeeds.
+   *
+   * <p>For example, DifferentialDriveWheelSpeeds{2.0, 2.5} / 2 = DifferentialDriveWheelSpeeds{1.0,
+   * 1.25}
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled DifferentialDriveWheelSpeeds.
+   */
+  public DifferentialDriveWheelSpeeds div(double scalar) {
+    return new DifferentialDriveWheelSpeeds(
+        leftMetersPerSecond / scalar, rightMetersPerSecond / scalar);
+  }
+
   @Override
   public String toString() {
     return String.format(

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

@@ -73,6 +73,89 @@ public class MecanumDriveWheelSpeeds {
     }
   }
 
+  /**
+   * Adds two MecanumDriveWheelSpeeds and returns the sum.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{1.0, 0.5, 2.0, 1.5} + MecanumDriveWheelSpeeds{2.0, 1.5,
+   * 0.5, 1.0} = MecanumDriveWheelSpeeds{3.0, 2.0, 2.5, 2.5}
+   *
+   * @param other The MecanumDriveWheelSpeeds to add.
+   * @return The sum of the MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds plus(MecanumDriveWheelSpeeds other) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond + other.frontLeftMetersPerSecond,
+        frontRightMetersPerSecond + other.frontRightMetersPerSecond,
+        rearLeftMetersPerSecond + other.rearLeftMetersPerSecond,
+        rearRightMetersPerSecond + other.rearRightMetersPerSecond);
+  }
+
+  /**
+   * Subtracts the other MecanumDriveWheelSpeeds from the current MecanumDriveWheelSpeeds and
+   * returns the difference.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{5.0, 4.0, 6.0, 2.5} - MecanumDriveWheelSpeeds{1.0, 2.0,
+   * 3.0, 0.5} = MecanumDriveWheelSpeeds{4.0, 2.0, 3.0, 2.0}
+   *
+   * @param other The MecanumDriveWheelSpeeds to subtract.
+   * @return The difference between the two MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds minus(MecanumDriveWheelSpeeds other) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond - other.frontLeftMetersPerSecond,
+        frontRightMetersPerSecond - other.frontRightMetersPerSecond,
+        rearLeftMetersPerSecond - other.rearLeftMetersPerSecond,
+        rearRightMetersPerSecond - other.rearRightMetersPerSecond);
+  }
+
+  /**
+   * Returns the inverse of the current MecanumDriveWheelSpeeds. This is equivalent to negating all
+   * components of the MecanumDriveWheelSpeeds.
+   *
+   * @return The inverse of the current MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds unaryMinus() {
+    return new MecanumDriveWheelSpeeds(
+        -frontLeftMetersPerSecond,
+        -frontRightMetersPerSecond,
+        -rearLeftMetersPerSecond,
+        -rearRightMetersPerSecond);
+  }
+
+  /**
+   * Multiplies the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 3.0, 3.5} * 2 = MecanumDriveWheelSpeeds{4.0,
+   * 5.0, 6.0, 7.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds times(double scalar) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond * scalar,
+        frontRightMetersPerSecond * scalar,
+        rearLeftMetersPerSecond * scalar,
+        rearRightMetersPerSecond * scalar);
+  }
+
+  /**
+   * Divides the MecanumDriveWheelSpeeds by a scalar and returns the new MecanumDriveWheelSpeeds.
+   *
+   * <p>For example, MecanumDriveWheelSpeeds{2.0, 2.5, 1.5, 1.0} / 2 = MecanumDriveWheelSpeeds{1.0,
+   * 1.25, 0.75, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled MecanumDriveWheelSpeeds.
+   */
+  public MecanumDriveWheelSpeeds div(double scalar) {
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond / scalar,
+        frontRightMetersPerSecond / scalar,
+        rearLeftMetersPerSecond / scalar,
+        rearRightMetersPerSecond / scalar);
+  }
+
   @Override
   public String toString() {
     return String.format(