Set up Mockito and simple Unit Test

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);
     }
 
     /**