Cleaned up Math, added unit testing to AimToCenter

src/test/java/frc4388/commands/AimToCenterTest.java

@@ -0,0 +1,113 @@
+package frc4388.commands;
+
+import org.junit.Test;
+
+import frc4388.robot.commands.AimToCenter;
+import org.junit.Assert;
+
+public class AimToCenterTest {
+
+    private static final double DELTA = 1e-15;
+
+    @Test
+    public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() {
+        boolean output;
+
+            //20 deg
+        output = AimToCenter.isDeadzone(20.);
+        Assert.assertFalse(output);
+
+            //-10 deg
+        output = AimToCenter.isDeadzone(-10.);
+        Assert.assertTrue(output);
+        
+            //-1 deg
+        output = AimToCenter.isDeadzone(-1.);
+        Assert.assertTrue(output);
+
+            //341 deg
+        output = AimToCenter.isDeadzone(341.);
+        Assert.assertTrue(output);
+
+            //340 deg
+        output = AimToCenter.isDeadzone(340.);
+        Assert.assertFalse(output);
+        
+            //0 deg
+        output = AimToCenter.isDeadzone(0.);
+        Assert.assertFalse(output);
+        
+            //200 deg
+        output = AimToCenter.isDeadzone(200.);
+        Assert.assertFalse(output);
+
+            //2000000 deg
+        output = AimToCenter.isDeadzone(2000000.);
+        Assert.assertTrue(output);
+
+            //NaN deg
+        output = AimToCenter.isDeadzone(Double.NaN);
+        Assert.assertFalse(output);
+    }
+
+    @Test 
+    public void givenOdometry_whenCalculateAngleToCenter_thenReturnAngleToCenter() {
+        double actual;
+        double expected;
+
+            //(5,5) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(5., 5., 0.);
+        expected = 180. + 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(-5,5) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(-5.0, 5., 0.);
+        expected = 180. + 90. + 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(-5,-5) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(-5.0, -5., 0.);
+        expected = 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(5,-5) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(5., -5., 0.);
+        expected = 90. + 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(0,0) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(0., 0., 0.);
+        Assert.assertNotNull(actual);
+
+            //(5,5) Gyro = 180 deg
+        actual = AimToCenter.angleToCenter(5., 5., 180.);
+        expected = 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(100,100) Gyro = 90 deg
+        actual = AimToCenter.angleToCenter(100., 100., 90.);
+        expected = 90. + 45.;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(null,5) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(Double.NaN, 5., 0.);
+        expected = Double.NaN;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(null,null) Gyro = 0 deg
+        actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, 0.);
+        expected = Double.NaN;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(null,null) Gyro = null deg
+        actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, Double.NaN);
+        expected = Double.NaN;
+        Assert.assertEquals(expected, actual, DELTA);
+
+            //(5,5) Gyro = -20 deg
+        actual = AimToCenter.angleToCenter(5., -5., -45.);
+        expected = 180.;
+        Assert.assertEquals(expected, actual, DELTA);
+    
+    }
+}

src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java

@@ -17,23 +17,17 @@ import frc4388.utility.LEDPatterns;
  * Add your docs here.
  */
 public class LEDSubsystemTest {
+  // Arrange
+  Spark ledController = mock(Spark.class);
+  LED led = new LED(ledController);
   @Test
   public void testConstructor() {
-    // Arrange
-    Spark ledController = mock(Spark.class);
-
-    // Act
-    LED led = new LED(ledController);
-
     // Assert
     assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
   }
 
   @Test
   public void testPatterns() {
-    // Arrange
-    Spark ledController = mock(Spark.class);
-    LED led = new LED(ledController);
 
     // Act
     led.setPattern(LEDPatterns.RAINBOW_RAINBOW);