Set up Mockito and simple Unit Test

build.gradle

@@ -53,6 +53,7 @@ dependencies {
     nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)
 
     testImplementation 'junit:junit:4.12'
+    testCompile "org.mockito:mockito-core:2.+"
 
     // Enable simulation gui support. Must check the box in vscode to enable support
     // upon debugging

src/main/java/frc4388/robot/RobotContainer.java

@@ -32,7 +32,7 @@ public class RobotContainer {
 
     /* Subsystems */
     private final Drive m_robotDrive = new Drive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor,
-            m_robotMap.leftBackMotor, m_robotMap.rightBackMotor, m_robotMap.gyroDrive);
+            m_robotMap.leftBackMotor, m_robotMap.rightBackMotor, m_robotMap.driveTrain, m_robotMap.gyroDrive);
 
     private final LED m_robotLED = new LED(m_robotMap.LEDController);
 

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -37,13 +37,13 @@ public class Drive extends SubsystemBase {
    * Add your docs here.
    */
   public Drive(WPI_TalonSRX leftFrontMotor, WPI_TalonSRX rightFrontMotor, WPI_TalonSRX leftBackMotor,
-      WPI_TalonSRX rightBackMotor, RobotGyro gyro) {
+      WPI_TalonSRX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
 
     m_leftFrontMotor = leftFrontMotor;
     m_rightFrontMotor = rightFrontMotor;
     m_leftBackMotor = leftBackMotor;
     m_rightBackMotor = rightBackMotor;
-    m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
+    m_driveTrain = driveTrain;
     m_gyro = gyro;
   }
 
@@ -63,6 +63,14 @@ public class Drive extends SubsystemBase {
     m_driveTrain.arcadeDrive(move, steer);
   }
 
+  /**
+   * Add your docs here.
+   */
+  public void tankDriveWithInput(double leftMove, double rightMove) {
+    m_leftFrontMotor.set(leftMove);
+    m_rightFrontMotor.set(rightMove);
+  }
+
   /**
    * Add your docs here.
    */

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

@@ -117,7 +117,15 @@ public class RobotGyro extends GyroBase {
      * @return heading from -180 to 180 degrees
      */
     public double getHeading() {
-        return Math.IEEEremainder(getAngle(), 360);
+        return getHeading(getAngle());
+    }
+
+    /**
+     * Gets an absolute heading of the robot
+     * @return heading from -180 to 180 degrees
+     */
+    public double getHeading(double angle) {
+        return Math.IEEEremainder(angle, 360);
     }
 
     /**

src/test/java/frc4388/robot/utility/GyroHeadingTest.java

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* 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.robot.utility;
+
+import static org.junit.Assert.*;
+import static org.mockito.Mockito.*;
+
+import com.ctre.phoenix.sensors.PigeonIMU;
+
+import edu.wpi.first.wpilibj.*;
+import frc4388.utility.RobotGyro;
+
+import org.junit.*;
+
+/**
+ * Add your docs here.
+ */
+public class GyroHeadingTest {
+
+    @Test
+    public void testConstructor() {
+        // Arrange
+        RobotGyro gyro = new RobotGyro(mock(PigeonIMU.class));
+
+        // Act & Assert
+        assertEquals(-90, gyro.getHeading(270), 0.0001);
+        assertEquals(-45, gyro.getHeading(315), 0.0001);
+        assertEquals(-60, gyro.getHeading(-60), 0.0001);
+        assertEquals(30, gyro.getHeading(30), 0.0001);
+        assertEquals(0, gyro.getHeading(0), 0.0001);
+        assertEquals(180, gyro.getHeading(180), 0.0001);
+        assertEquals(-180, gyro.getHeading(-180), 0.0001);
+        assertEquals(97, gyro.getHeading(1537), 0.0001);
+        assertEquals(99, gyro.getHeading(-2781), 0.0001);
+    }
+}