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

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

wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java

@@ -23,4 +23,60 @@ class ChassisSpeedsTest {
         () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
         () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
   }
+
+  @Test
+  void testPlus() {
+    final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
+    final var right = new ChassisSpeeds(2.0, 1.5, 0.25);
+
+    final var chassisSpeeds = left.plus(right);
+
+    assertAll(
+        () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(2.0, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(1.0, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testMinus() {
+    final var left = new ChassisSpeeds(1.0, 0.5, 0.75);
+    final var right = new ChassisSpeeds(2.0, 0.5, 0.25);
+
+    final var chassisSpeeds = left.minus(right);
+
+    assertAll(
+        () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testUnaryMinus() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).unaryMinus();
+
+    assertAll(
+        () -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(-0.5, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(-0.75, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testMultiplication() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).times(2.0);
+
+    assertAll(
+        () -> assertEquals(2.0, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond));
+  }
+
+  @Test
+  void testDivision() {
+    final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).div(2.0);
+
+    assertAll(
+        () -> assertEquals(0.5, chassisSpeeds.vxMetersPerSecond),
+        () -> assertEquals(0.25, chassisSpeeds.vyMetersPerSecond),
+        () -> assertEquals(0.375, chassisSpeeds.omegaRadiansPerSecond));
+  }
 }