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

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