From 489d4a5bff450f5053283d294baf62051a538a92 Mon Sep 17 00:00:00 2001
From: "Keenan D. Buckley" <keenandbuckley@protonmail.com>
Date: Sat, 28 Mar 2020 22:45:44 -0600
Subject: [PATCH] Set up Mockito and simple Unit Test

---
 build.gradle                                  |  1 +
 .../java/frc4388/robot/RobotContainer.java    |  2 +-
 .../java/frc4388/robot/subsystems/Drive.java  | 12 +++++-
 src/main/java/frc4388/utility/RobotGyro.java  | 10 ++++-
 .../robot/utility/GyroHeadingTest.java        | 41 +++++++++++++++++++
 5 files changed, 62 insertions(+), 4 deletions(-)
 create mode 100644 src/test/java/frc4388/robot/utility/GyroHeadingTest.java

diff --git a/build.gradle b/build.gradle
index 434f06c..b202c6b 100644
--- a/build.gradle
+++ b/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
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 381910c..b7542bc 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/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);
 
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index b32547c..1703513 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/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.
    */
diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java
index ab66d00..6d5ba5c 100644
--- a/src/main/java/frc4388/utility/RobotGyro.java
+++ b/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);
     }
 
     /**
diff --git a/src/test/java/frc4388/robot/utility/GyroHeadingTest.java b/src/test/java/frc4388/robot/utility/GyroHeadingTest.java
new file mode 100644
index 0000000..396a615
--- /dev/null
+++ b/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);
+    }
+}