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 AHRS m_navX;
+    private PigeonIMU m_pigeon = null;
+    private AHRS m_navX = null;
     public boolean m_isGyroAPigeon; //true if pigeon, false if navX
 
     private double m_lastPigeonAngle;