Create Mock PigeonIMU and write more Unit tests

- Test to make sure the RoboyGyro class knows what kind of gyro its using
- Test to make sure all the getters for the gyro class work (at least for pigeon)

src/main/java/frc4388/mocks/MockPigeonIMU.java

@@ -0,0 +1,54 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.mocks;
+
+import com.ctre.phoenix.ErrorCode;
+import com.ctre.phoenix.sensors.PigeonIMU;
+
+/**
+ * Add your docs here.
+ */
+public class MockPigeonIMU extends PigeonIMU {
+    public int m_deviceNumber;
+    public double currentYaw;
+    public double currentPitch;
+    public double currentRoll;
+
+    public MockPigeonIMU(int deviceNumber) {
+        super(deviceNumber);
+        m_deviceNumber = deviceNumber;
+    }
+
+    @Override
+    public ErrorCode setYaw(double angleDeg) {
+        currentYaw = angleDeg;
+        return ErrorCode.OK;
+    }
+
+    /**
+     * @param currentPitch the Pitch to set
+     */
+    public void setCurrentPitch(double currentPitch) {
+        this.currentPitch = currentPitch;
+    }
+
+    /**
+     * @param currentRoll the Roll to set
+     */
+    public void setCurrentRoll(double currentRoll) {
+        this.currentRoll = currentRoll;
+    }
+
+    @Override
+    public ErrorCode getYawPitchRoll(double[] ypr_deg) {
+        ypr_deg[0] = currentYaw;
+        ypr_deg[1] = currentPitch;
+        ypr_deg[2] = currentRoll;
+        return ErrorCode.OK;
+    }
+}

src/main/java/frc4388/utility/RobotGyro.java

@@ -20,8 +20,8 @@ import edu.wpi.first.wpiutil.math.MathUtil;
 public class RobotGyro extends GyroBase {
     private RobotTime m_robotTime = RobotTime.getInstance();
 
-    private PigeonIMU m_pigeon;
+    private PigeonIMU m_pigeon = null;
-    private AHRS m_navX;
+    private AHRS m_navX = null;
     public boolean m_isGyroAPigeon; //true if pigeon, false if navX
 
     private double m_lastPigeonAngle;

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

@@ -31,12 +31,6 @@ public class LEDSubsystemTest {
         led.setPattern(LEDPatterns.BLUE_BREATH);
         assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
 
-        led.setPattern(LEDPatterns.C1_SCANNER);
-        assertEquals(LEDPatterns.C1_SCANNER.getValue(), led.getPattern().getValue(), 0.0001);
-
-        led.setPattern(LEDPatterns.C1_CHASE);
-        assertEquals(LEDPatterns.C1_CHASE.getValue(), led.getPattern().getValue(), 0.0001);
-
         led.setPattern(LEDPatterns.SOLID_BLACK);
         assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
     }

src/test/java/frc4388/utility/RobotGyroUtilityTest.java

@@ -13,26 +13,87 @@ import org.junit.*;
 
 import edu.wpi.first.wpilibj.*;
 import com.ctre.phoenix.sensors.PigeonIMU;
+import com.kauailabs.navx.frc.AHRS;
+
+import frc4388.mocks.MockPigeonIMU;
+import frc4388.robot.Constants.DriveConstants;
 import frc4388.utility.RobotGyro;
 
 /**
  * Add your docs here.
  */
 public class RobotGyroUtilityTest {
-    // Arrange
+    MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
-    RobotGyro gyro = new RobotGyro(mock(PigeonIMU.class));
+    RobotGyro gyroPigeon = new RobotGyro(pigeon);
+    AHRS navX = mock(AHRS.class);
+    RobotGyro gyroNavX = new RobotGyro(navX);
+
+    // TODO UNTESTED: calibrate(), getRate(), and most functions for NavX
+
+    @Test
+    public void testConfig() {
+        assertEquals(true, gyroPigeon.m_isGyroAPigeon);
+        assertEquals(pigeon, gyroPigeon.getPigeon());
+        assertEquals(null, gyroPigeon.getNavX());
+        assertEquals(false, gyroNavX.m_isGyroAPigeon);
+        assertEquals(navX, gyroNavX.getNavX());
+        assertEquals(null, gyroNavX.getPigeon());
+    }
 
     @Test
     public void testHeading() {
         // Act & Assert
-        assertEquals(-90, gyro.getHeading(270), 0.0001);
+        assertEquals(-90, gyroPigeon.getHeading(270), 0.0001);
-        assertEquals(-45, gyro.getHeading(315), 0.0001);
+        assertEquals(-45, gyroPigeon.getHeading(315), 0.0001);
-        assertEquals(-60, gyro.getHeading(-60), 0.0001);
+        assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001);
-        assertEquals(30, gyro.getHeading(30), 0.0001);
+        assertEquals(30, gyroPigeon.getHeading(30), 0.0001);
-        assertEquals(0, gyro.getHeading(0), 0.0001);
+        assertEquals(0, gyroPigeon.getHeading(0), 0.0001);
-        assertEquals(180, gyro.getHeading(180), 0.0001);
+        assertEquals(180, gyroPigeon.getHeading(180), 0.0001);
-        assertEquals(-180, gyro.getHeading(-180), 0.0001);
+        assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001);
-        assertEquals(97, gyro.getHeading(1537), 0.0001);
+        assertEquals(97, gyroPigeon.getHeading(1537), 0.0001);
-        assertEquals(99, gyro.getHeading(-2781), 0.0001);
+        assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001);
+    }
+
+    @Test
+    public void testYawPitchRoll() {
+        assertEquals(0, gyroPigeon.getAngle(), 0.0001);
+
+        pigeon.setYaw(40);
+        assertEquals(40, gyroPigeon.getAngle(), 0.0001);
+
+        gyroPigeon.reset();
+        assertEquals(0, gyroPigeon.getAngle(), 0.0001);
+
+        pigeon.setYaw(-1457);
+        pigeon.setCurrentPitch(100);
+        pigeon.setCurrentRoll(100);
+        assertEquals(-1457, gyroPigeon.getAngle(), 0.0001);
+        assertEquals(90, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(90, gyroPigeon.getRoll(), 0.0001);
+
+        pigeon.setCurrentPitch(45);
+        pigeon.setCurrentRoll(45);
+        assertEquals(45, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(45, gyroPigeon.getRoll(), 0.0001);
+
+        pigeon.setCurrentPitch(0);
+        pigeon.setCurrentRoll(0);
+        assertEquals(0, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(0, gyroPigeon.getRoll(), 0.0001);
+
+        pigeon.setCurrentPitch(-60);
+        pigeon.setCurrentRoll(-60);
+        assertEquals(-60, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(-60, gyroPigeon.getRoll(), 0.0001);
+
+        pigeon.setCurrentPitch(-90);
+        pigeon.setCurrentRoll(-90);
+        assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
+
+        pigeon.setCurrentPitch(-100);
+        pigeon.setCurrentRoll(-100);
+        assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
     }
 }