[wpimath] ChassisSpeeds: Add arithmetic functions (#5293)

Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>

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

@@ -89,6 +89,74 @@ public class ChassisSpeeds {
         robotAngle);
   }
 
+  /**
+   * Adds two ChassisSpeeds and returns the sum.
+   *
+   * <p>For example, ChassisSpeeds{1.0, 0.5, 0.75} + ChassisSpeeds{2.0, 1.5, 0.25} =
+   * ChassisSpeeds{3.0, 2.0, 1.0}
+   *
+   * @param other The ChassisSpeeds to add.
+   * @return The sum of the ChassisSpeeds.
+   */
+  public ChassisSpeeds plus(ChassisSpeeds other) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond + other.vxMetersPerSecond,
+        vyMetersPerSecond + other.vyMetersPerSecond,
+        omegaRadiansPerSecond + other.omegaRadiansPerSecond);
+  }
+
+  /**
+   * Subtracts the other ChassisSpeeds from the other 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}
+   *
+   * @param other The ChassisSpeeds to subtract.
+   * @return The difference between the two ChassisSpeeds.
+   */
+  public ChassisSpeeds minus(ChassisSpeeds other) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond - other.vxMetersPerSecond,
+        vyMetersPerSecond - other.vyMetersPerSecond,
+        omegaRadiansPerSecond - other.omegaRadiansPerSecond);
+  }
+
+  /**
+   * Returns the inverse of the current ChassisSpeeds. This is equivalent to negating all components
+   * of the ChassisSpeeds.
+   *
+   * @return The inverse of the current ChassisSpeeds.
+   */
+  public ChassisSpeeds unaryMinus() {
+    return new ChassisSpeeds(-vxMetersPerSecond, -vyMetersPerSecond, -omegaRadiansPerSecond);
+  }
+
+  /**
+   * Multiplies the ChassisSpeeds by a scalar and returns the new ChassisSpeeds.
+   *
+   * <p>For example, ChassisSpeeds{2.0, 2.5, 1.0} * 2 = ChassisSpeeds{4.0, 5.0, 1.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled ChassisSpeeds.
+   */
+  public ChassisSpeeds times(double scalar) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond * scalar, vyMetersPerSecond * scalar, omegaRadiansPerSecond * scalar);
+  }
+
+  /**
+   * Divides the ChassisSpeeds by a scalar and returns the new 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.
+   */
+  public ChassisSpeeds div(double scalar) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond / scalar, vyMetersPerSecond / scalar, omegaRadiansPerSecond / scalar);
+  }
+
   @Override
   public String toString() {
     return String.format(