[wpimath] Add arithmetic functions to wheel speeds classes (wpilibsui…

…te#5465)

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

@@ -154,7 +154,7 @@ public ChassisSpeeds plus(ChassisSpeeds other) {
   }
 
   /**
-   * 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 ChassisSpeeds times(double scalar) {
    *
    * <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 void desaturate(double attainableMaxSpeedMetersPerSecond) {
     }
   }
 
+  /**
+   * 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 void desaturate(double attainableMaxSpeedMetersPerSecond) {
     }
   }
 
+  /**
+   * 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(

wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h

@@ -146,8 +146,8 @@ struct WPILIB_DLLEXPORT 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}
@@ -188,9 +188,9 @@ struct WPILIB_DLLEXPORT 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.
+   * @param scalar The scalar to divide by.
    *
-   * @return The reference to the new mutated object.
+   * @return The scaled ChassisSpeeds.
    */
   constexpr ChassisSpeeds operator/(double scalar) const {
     return operator*(1.0 / scalar);

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h

@@ -36,5 +36,79 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
    * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
    */
   void Desaturate(units::meters_per_second_t attainableMaxSpeed);
+
+  /**
+   * 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.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator+(
+      const DifferentialDriveWheelSpeeds& other) const {
+    return {left + other.left, right + other.right};
+  }
+
+  /**
+   * 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.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator-(
+      const DifferentialDriveWheelSpeeds& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current DifferentialDriveWheelSpeeds.
+   * This is equivalent to negating all components of the
+   * DifferentialDriveWheelSpeeds.
+   *
+   * @return The inverse of the current DifferentialDriveWheelSpeeds.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator-() const {
+    return {-left, -right};
+  }
+
+  /**
+   * 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.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator*(double scalar) const {
+    return {scalar * left, scalar * right};
+  }
+
+  /**
+   * 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.
+   */
+  constexpr DifferentialDriveWheelSpeeds operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
 };
 }  // namespace frc

wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h

@@ -46,5 +46,77 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
    * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
    */
   void Desaturate(units::meters_per_second_t attainableMaxSpeed);
+
+  /**
+   * 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.
+   */
+  constexpr MecanumDriveWheelSpeeds operator+(
+      const MecanumDriveWheelSpeeds& other) const {
+    return {frontLeft + other.frontLeft, frontRight + other.frontRight,
+            rearLeft + other.rearLeft, rearRight + other.rearRight};
+  }
+
+  /**
+   * 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.
+   */
+  constexpr MecanumDriveWheelSpeeds operator-(
+      const MecanumDriveWheelSpeeds& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current MecanumDriveWheelSpeeds.
+   * This is equivalent to negating all components of the
+   * MecanumDriveWheelSpeeds.
+   *
+   * @return The inverse of the current MecanumDriveWheelSpeeds.
+   */
+  constexpr MecanumDriveWheelSpeeds operator-() const {
+    return {-frontLeft, -frontRight, -rearLeft, -rearRight};
+  }
+
+  /**
+   * 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.
+   */
+  constexpr MecanumDriveWheelSpeeds operator*(double scalar) const {
+    return {scalar * frontLeft, scalar * frontRight, scalar * rearLeft,
+            scalar * rearRight};
+  }
+
+  /**
+   * 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.
+   */
+  constexpr MecanumDriveWheelSpeeds operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
 };
 }  // namespace frc