Merge pull request #17 from Team4388/dependency-injection

Setup Unit Testing

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/Robot.java

@@ -22,6 +22,7 @@ import frc4388.utility.RobotTime;
 public class Robot extends TimedRobot {
   Command m_autonomousCommand;
   
+  private RobotTime m_robotTime = RobotTime.getInstance();
   private RobotContainer m_robotContainer;
 
   /**
@@ -45,7 +46,7 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void robotPeriodic() {
-    RobotTime.updateTimes();
+    m_robotTime.updateTimes();
     // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
     // commands, running already-scheduled commands, removing finished or interrupted commands,
     // and running subsystem periodic() methods.  This must be called from the robot's periodic
@@ -60,7 +61,7 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void disabledInit() {
-    RobotTime.endMatchTime();
+    m_robotTime.endMatchTime();
   }
 
   @Override
@@ -85,7 +86,7 @@ public class Robot extends TimedRobot {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.schedule();
     }
-    RobotTime.startMatchTime();
+    m_robotTime.startMatchTime();
   }
 
   /**
@@ -104,7 +105,7 @@ public class Robot extends TimedRobot {
     if (m_autonomousCommand != null) {
       m_autonomousCommand.cancel();
     }
-    RobotTime.startMatchTime();
+    m_robotTime.startMatchTime();
   }
 
   /**

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

@@ -27,46 +27,51 @@ import frc4388.utility.controller.XboxController;
  * commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
+    /* RobotMap */
+    private final RobotMap m_robotMap = new RobotMap();
+
     /* Subsystems */
-    private final Drive m_robotDrive = new Drive();
+    private final Drive m_robotDrive = new Drive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor,
-    private final LED m_robotLED = new LED();
+            m_robotMap.leftBackMotor, m_robotMap.rightBackMotor, m_robotMap.driveTrain, m_robotMap.gyroDrive);
+
+    private final LED m_robotLED = new LED(m_robotMap.LEDController);
 
     /* Controllers */
     private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
     private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
 
     /**
-     * The container for the robot.  Contains subsystems, OI devices, and commands.
+     * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
         configureButtonBindings();
 
         /* Default Commands */
         // drives the robot with a two-axis input from the driver controller
-        m_robotDrive.setDefaultCommand(new RunCommand(() -> m_robotDrive.driveWithInput(
+        m_robotDrive.setDefaultCommand(
-            getDriverController().getLeftYAxis(),
+                new RunCommand(() -> m_robotDrive.driveWithInput(getDriverController().getLeftYAxis(),
-            getDriverController().getRightXAxis()), m_robotDrive));
+                        getDriverController().getRightXAxis()), m_robotDrive));
         // continually sends updates to the Blinkin LED controller to keep the lights on
         m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED));
     }
 
     /**
-    * Use this method to define your button->command mappings.  Buttons can be created by
+     * Use this method to define your button->command mappings. Buttons can be
-    * instantiating a {@link GenericHID} or one of its subclasses ({@link
+     * created by instantiating a {@link GenericHID} or one of its subclasses
-    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
+     * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
-    * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+     * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
-    */
+     */
     private void configureButtonBindings() {
         /* Driver Buttons */
         // test command to spin the robot while pressing A on the driver controller
         new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
-            .whileHeld(() -> m_robotDrive.driveWithInput(0, 1));
+                .whileHeld(() -> m_robotDrive.driveWithInput(0, 1));
 
         /* Operator Buttons */
         // activates "Lit Mode"
         new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
-            .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
+                .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
-            .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+                .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
     }
 
     /**
@@ -89,24 +94,21 @@ public class RobotContainer {
     /**
      * Add your docs here.
      */
-    public IHandController getOperatorController()
+    public IHandController getOperatorController() {
-    {
         return m_operatorXbox;
     }
 
     /**
      * Add your docs here.
      */
-    public Joystick getOperatorJoystick()
+    public Joystick getOperatorJoystick() {
-    {
         return m_operatorXbox.getJoyStick();
     }
 
     /**
      * Add your docs here.
      */
-    public Joystick getDriverJoystick()
+    public Joystick getDriverJoystick() {
-    {
         return m_driverXbox.getJoyStick();
     }
 }

src/main/java/frc4388/robot/RobotMap.java

@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+import com.ctre.phoenix.motorcontrol.InvertType;
+import com.ctre.phoenix.motorcontrol.NeutralMode;
+import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
+import com.ctre.phoenix.sensors.PigeonIMU;
+
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.RobotGyro;
+
+/**
+ * Defines and holds all I/O objects on the Roborio. This is useful for unit
+ * testing and modularization.
+ */
+public class RobotMap {
+
+    public RobotMap() {
+        configureLEDMotorControllers();
+        configureDriveMotorControllers();
+    }
+
+    /* LED Subsystem */
+    public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+
+    void configureLEDMotorControllers() {
+        
+    }
+
+    /* Drive Subsystem */
+    public final WPI_TalonSRX leftFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
+    public final WPI_TalonSRX rightFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
+    public final WPI_TalonSRX leftBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
+    public final WPI_TalonSRX rightBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
+    public final DifferentialDrive driveTrain = new DifferentialDrive(leftFrontMotor, rightFrontMotor);
+    public final RobotGyro gyroDrive = new RobotGyro(new PigeonIMU(DriveConstants.DRIVE_PIGEON_ID));
+
+    void configureDriveMotorControllers() {
+
+        /* factory default values */
+        leftFrontMotor.configFactoryDefault();
+        rightFrontMotor.configFactoryDefault();
+        leftBackMotor.configFactoryDefault();
+        rightBackMotor.configFactoryDefault();
+
+        /* set back motors as followers */
+        leftBackMotor.follow(leftFrontMotor);
+        rightBackMotor.follow(rightFrontMotor);
+
+        /* set neutral mode */
+        leftFrontMotor.setNeutralMode(NeutralMode.Brake);
+        rightFrontMotor.setNeutralMode(NeutralMode.Brake);
+        leftFrontMotor.setNeutralMode(NeutralMode.Brake);
+        rightFrontMotor.setNeutralMode(NeutralMode.Brake);
+
+        /* flip input so forward becomes back, etc */
+        leftFrontMotor.setInverted(false);
+        rightFrontMotor.setInverted(false);
+        leftBackMotor.setInverted(InvertType.FollowMaster);
+        rightBackMotor.setInverted(InvertType.FollowMaster);
+    }
+}

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

@@ -7,10 +7,7 @@
 
 package frc4388.robot.subsystems;
 
-import com.ctre.phoenix.motorcontrol.InvertType;
-import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-import com.ctre.phoenix.sensors.PigeonIMU;
 
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -27,45 +24,34 @@ public class Drive extends SubsystemBase {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
 
-  private WPI_TalonSRX m_leftFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
+  private RobotTime m_robotTime = RobotTime.getInstance();
-  private WPI_TalonSRX m_rightFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
+
-  private WPI_TalonSRX m_leftBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
+  private WPI_TalonSRX m_leftFrontMotor;
-  private WPI_TalonSRX m_rightBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
+  private WPI_TalonSRX m_rightFrontMotor;
-  private DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
+  private WPI_TalonSRX m_leftBackMotor;
-  private RobotGyro m_gyro = new RobotGyro(new PigeonIMU(DriveConstants.DRIVE_PIGEON_ID));
+  private WPI_TalonSRX m_rightBackMotor;
+  private DifferentialDrive m_driveTrain;
+  private RobotGyro m_gyro;
 
   /**
    * Add your docs here.
    */
-  public Drive(){
+  public Drive(WPI_TalonSRX leftFrontMotor, WPI_TalonSRX rightFrontMotor, WPI_TalonSRX leftBackMotor,
-    /* factory default values */
+      WPI_TalonSRX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
-    m_leftFrontMotor.configFactoryDefault();
-    m_rightFrontMotor.configFactoryDefault();
-    m_leftBackMotor.configFactoryDefault();
-    m_rightBackMotor.configFactoryDefault();
 
-    /* set back motors as followers */
+    m_leftFrontMotor = leftFrontMotor;
-    m_leftBackMotor.follow(m_leftFrontMotor);
+    m_rightFrontMotor = rightFrontMotor;
-    m_rightBackMotor.follow(m_rightFrontMotor);
+    m_leftBackMotor = leftBackMotor;
-
+    m_rightBackMotor = rightBackMotor;
-    /* set neutral mode */
+    m_driveTrain = driveTrain;
-    m_leftFrontMotor.setNeutralMode(NeutralMode.Brake);
+    m_gyro = gyro;
-    m_rightFrontMotor.setNeutralMode(NeutralMode.Brake);
-    m_leftFrontMotor.setNeutralMode(NeutralMode.Brake);
-    m_rightFrontMotor.setNeutralMode(NeutralMode.Brake);
-
-    /* flip input so forward becomes back, etc */
-    m_leftFrontMotor.setInverted(false);
-    m_rightFrontMotor.setInverted(false);
-    m_leftBackMotor.setInverted(InvertType.FollowMaster);
-    m_rightBackMotor.setInverted(InvertType.FollowMaster);
   }
 
   @Override
   public void periodic() {
     m_gyro.updatePigeonDeltas();
 
-    if (RobotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
+    if (m_robotTime.m_frameNumber % DriveConstants.SMARTDASHBOARD_UPDATE_FRAME == 0) {
       updateSmartDashboard();
     }
   }
@@ -73,20 +59,27 @@ public class Drive extends SubsystemBase {
   /**
    * Add your docs here.
    */
-  public void driveWithInput(double move, double steer){
+  public void driveWithInput(double move, double steer) {
     m_driveTrain.arcadeDrive(move, steer);
   }
 
-  public RobotGyro getRobotGyro(){
+  /**
-    return m_gyro;
+   * Add your docs here.
+   */
+  public void tankDriveWithInput(double leftMove, double rightMove) {
+    m_leftFrontMotor.set(leftMove);
+    m_rightFrontMotor.set(rightMove);
   }
 
+  /**
+   * Add your docs here.
+   */
   private void updateSmartDashboard() {
 
     // Examples of the functionality of RobotGyro
-    SmartDashboard.putBoolean("Is Gyro a Pigeon?", getRobotGyro().m_isGyroAPigeon);
+    SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
-    SmartDashboard.putNumber("Turn Rate", getRobotGyro().getRate());
+    SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
-    SmartDashboard.putNumber("Gyro Pitch", getRobotGyro().getPitch());
+    SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
-    SmartDashboard.putData(getRobotGyro());
+    SmartDashboard.putData(m_gyro);
   }
 }

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

@@ -20,36 +20,44 @@ import frc4388.utility.LEDPatterns;
  */
 public class LED extends SubsystemBase {
 
-  private float currentLED;
+  private LEDPatterns m_currentPattern;
-  private Spark LEDController;
+  private Spark m_LEDController;
 
   /**
    * Add your docs here.
    */
-  public LED(){
+  public LED(Spark LEDController){
-    LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+    m_LEDController = LEDController;
     setPattern(LEDConstants.DEFAULT_PATTERN);
-    LEDController.set(currentLED);
+    updateLED();
     System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
   }
 
+  @Override
+  public void periodic(){
+    SmartDashboard.putNumber("LED", m_currentPattern.getValue());
+  }
+
   /**
    * Add your docs here.
    */
   public void updateLED(){
-    LEDController.set(currentLED);
+    m_LEDController.set(m_currentPattern.getValue());
   }
 
   /**
    * Add your docs here.
    */
   public void setPattern(LEDPatterns pattern){
-    currentLED = pattern.getValue();
+    m_currentPattern = pattern;
-    LEDController.set(currentLED);
+    m_LEDController.set(m_currentPattern.getValue());
   }
 
-  @Override
+  /**
-  public void periodic(){
+   * Add your docs here.
-    SmartDashboard.putNumber("LED", currentLED);
+   * @return
+   */
+  public LEDPatterns getPattern() {
+    return m_currentPattern;
   }
 }

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

@@ -18,8 +18,10 @@ import edu.wpi.first.wpiutil.math.MathUtil;
  * Gyro class that allows for interchangeable use between a pigeon and a navX
  */
 public class RobotGyro extends GyroBase {
-    private PigeonIMU m_pigeon;
+    private RobotTime m_robotTime = RobotTime.getInstance();
-    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;
@@ -115,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);
     }
 
     /**
@@ -149,7 +159,7 @@ public class RobotGyro extends GyroBase {
     @Override
     public double getRate() {
         if (m_isGyroAPigeon) {
-            return m_deltaPigeonAngle / (RobotTime.m_deltaTime * 1000);
+            return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000;
         } else {
             return m_navX.getRate();
         }

src/main/java/frc4388/utility/RobotTime.java

@@ -11,29 +11,42 @@ package frc4388.utility;
  * <p>Keeps track of Robot times like time passed, delta time, etc
  * <p>All times are in milliseconds
  */
-public final class RobotTime {
+public class RobotTime {
-    private static long m_currTime = System.currentTimeMillis();
+    private long m_currTime = System.currentTimeMillis();
-    public static long m_deltaTime = 0;
+    public long m_deltaTime = 0;
 
-    private static long m_startRobotTime = m_currTime;
+    private long m_startRobotTime = m_currTime;
-    public static long m_robotTime = 0;
+    public long m_robotTime = 0;
-    public static long m_lastRobotTime = 0;
+    public long m_lastRobotTime = 0;
 
-    private static long m_startMatchTime = 0;
+    private long m_startMatchTime = 0;
-    public static long m_matchTime = 0;
+    public  long m_matchTime = 0;
-    public static long m_lastMatchTime = 0;
+    public long m_lastMatchTime = 0;
 
-    public static long m_frameNumber = 0;
+    public long m_frameNumber = 0;
 
     /**
-     * This class should not be instantiated.
+     * Private constructor prevents other classes from instantiating
      */
     private RobotTime(){}
 
+    private static RobotTime instance = null;
+
+    /**
+     * Gets the instance of Robot Time. If there is no instance running one will be created.
+     * @return instance of Robot Time
+     */
+    public static RobotTime getInstance() {
+        if (instance == null) {
+            instance = new RobotTime();
+        }
+        return instance;
+    }
+
     /**
      * Call this once per periodic loop.
      */
-    public static void updateTimes() {
+    public void updateTimes() {
         m_lastRobotTime = m_robotTime;
         m_lastMatchTime = m_matchTime;
 
@@ -42,7 +55,7 @@ public final class RobotTime {
         m_deltaTime = m_robotTime - m_lastRobotTime;
         m_frameNumber++;
 
-        if (m_matchTime != 0) {
+        if (m_startMatchTime != 0) {
             m_matchTime = m_currTime - m_startMatchTime;
         }
     }
@@ -50,17 +63,16 @@ public final class RobotTime {
     /**
      * Call this in both the auto and periodic inits
      */
-    public static void startMatchTime() {
+    public void startMatchTime() {
-        if (m_matchTime == 0) {
+        if (m_startMatchTime == 0) {
             m_startMatchTime = m_currTime;
-            m_matchTime = 1;
         }
     }
 
     /**
      * Call this in disabled init
      */
-    public static void endMatchTime() {
+    public void endMatchTime() {
         m_startMatchTime = 0;
         m_matchTime = 0;
     }

src/test/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/test/java/frc4388/robot/subsystems/LEDSubsystemTest.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.subsystems;
+
+import static org.junit.Assert.*;
+import static org.mockito.Mockito.*;
+import org.junit.*;
+import edu.wpi.first.wpilibj.*;
+import frc4388.robot.Constants.LEDConstants;
+import frc4388.utility.LEDPatterns;
+
+/**
+ * Add your docs here.
+ */
+public class LEDSubsystemTest {
+    Spark ledController = mock(Spark.class);
+    LED led = new LED(ledController);
+
+    @Test
+    public void testPatterns() {
+        // TEST 1
+        assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001);
+
+        // TEST 2
+        led.setPattern(LEDPatterns.RAINBOW_RAINBOW);
+        assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001);
+
+        // TEST 3
+        led.setPattern(LEDPatterns.BLUE_BREATH);
+        assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001);
+
+        // TEST 4
+        led.setPattern(LEDPatterns.SOLID_BLACK);
+        assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001);
+    }
+}

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

@@ -0,0 +1,155 @@
+/*----------------------------------------------------------------------------*/
+/* 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.utility;
+
+import static org.junit.Assert.*;
+import static org.mockito.Mockito.*;
+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 {
+    MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID);
+    RobotGyro gyroPigeon = new RobotGyro(pigeon);
+    AHRS navX = mock(AHRS.class);
+    RobotGyro gyroNavX = new RobotGyro(navX);
+    RobotTime robotTime = RobotTime.getInstance();
+
+    // TODO UNTESTED: most functions for NavX
+
+    @Test
+    public void testConfig() {
+        // TEST 1
+        assertEquals(true, gyroPigeon.m_isGyroAPigeon);
+        assertEquals(pigeon, gyroPigeon.getPigeon());
+        assertEquals(null, gyroPigeon.getNavX());
+
+        // TEST 2
+        assertEquals(false, gyroNavX.m_isGyroAPigeon);
+        assertEquals(navX, gyroNavX.getNavX());
+        assertEquals(null, gyroNavX.getPigeon());
+    }
+
+    @Test
+    public void testHeading() {
+        // TESTS
+        assertEquals(-90, gyroPigeon.getHeading(270), 0.0001);
+        assertEquals(-45, gyroPigeon.getHeading(315), 0.0001);
+        assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001);
+        assertEquals(30, gyroPigeon.getHeading(30), 0.0001);
+        assertEquals(0, gyroPigeon.getHeading(0), 0.0001);
+        assertEquals(180, gyroPigeon.getHeading(180), 0.0001);
+        assertEquals(-180, gyroPigeon.getHeading(-180), 0.0001);
+        assertEquals(97, gyroPigeon.getHeading(1537), 0.0001);
+        assertEquals(99, gyroPigeon.getHeading(-2781), 0.0001);
+    }
+
+    @Test
+    public void testYawPitchRoll() {        
+        // TEST 1
+        assertEquals(0, gyroPigeon.getAngle(), 0.0001);
+
+        // TEST 2
+        pigeon.setYaw(40);
+        assertEquals(40, gyroPigeon.getAngle(), 0.0001);
+
+        // TEST 3
+        gyroPigeon.reset();
+        assertEquals(0, gyroPigeon.getAngle(), 0.0001);
+
+        // TEST 4
+        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);
+
+        // TEST 5
+        pigeon.setCurrentPitch(45);
+        pigeon.setCurrentRoll(45);
+        assertEquals(45, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(45, gyroPigeon.getRoll(), 0.0001);
+
+        // TEST 6
+        pigeon.setCurrentPitch(0);
+        pigeon.setCurrentRoll(0);
+        assertEquals(0, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(0, gyroPigeon.getRoll(), 0.0001);
+
+        // TEST 7
+        pigeon.setCurrentPitch(-60);
+        pigeon.setCurrentRoll(-60);
+        assertEquals(-60, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(-60, gyroPigeon.getRoll(), 0.0001);
+
+        // TEST 8
+        pigeon.setCurrentPitch(-90);
+        pigeon.setCurrentRoll(-90);
+        assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
+
+        // TEST 9
+        pigeon.setCurrentPitch(-100);
+        pigeon.setCurrentRoll(-100);
+        assertEquals(-90, gyroPigeon.getPitch(), 0.0001);
+        assertEquals(-90, gyroPigeon.getRoll(), 0.0001);
+    }
+
+    @Test
+    public void testRates() {
+        // SETUP
+        pigeon.setYaw(0);
+        gyroPigeon.updatePigeonDeltas();
+
+        // TEST 1
+        robotTime.m_deltaTime = 5;
+        pigeon.setYaw(0);
+        gyroPigeon.updatePigeonDeltas();
+        assertEquals(0, gyroPigeon.getRate(), 1);
+
+        // TEST 2
+        robotTime.m_deltaTime = 5;
+        pigeon.setYaw(90);
+        gyroPigeon.updatePigeonDeltas();
+        assertEquals(18000, gyroPigeon.getRate(), 0.001);
+
+        // TEST 3
+        robotTime.m_deltaTime = 5;
+        pigeon.setYaw(90);
+        gyroPigeon.updatePigeonDeltas();
+        assertEquals(0, gyroPigeon.getRate(), 0.001);
+
+        // TEST 4
+        robotTime.m_deltaTime = 3;
+        pigeon.setYaw(-30);
+        gyroPigeon.updatePigeonDeltas();
+        assertEquals(-40000, gyroPigeon.getRate(), 0.001);
+
+        // TEST 5
+        robotTime.m_deltaTime = 6;
+        pigeon.setYaw(690);
+        gyroPigeon.updatePigeonDeltas();
+        assertEquals(120000, gyroPigeon.getRate(), 0.001);
+    }
+
+    private void wait(int millis) {
+        try {
+            Thread.sleep(millis);
+        } catch (Exception e) {}
+    }
+}

src/test/java/frc4388/utility/RobotTimeUtilityTest.java

@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* 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.utility;
+
+import static org.junit.Assert.*;
+import static org.mockito.Mockito.*;
+import org.junit.*;
+
+import edu.wpi.first.wpilibj.*;
+
+/**
+ * Add your docs here.
+ */
+public class RobotTimeUtilityTest {
+    RobotTime robotTime = RobotTime.getInstance();
+
+    @Test
+    public void testUpdateTimes() {
+        // SETUP
+        long lastTime;
+        robotTime.m_deltaTime = 0;
+        robotTime.m_robotTime = 0;
+        robotTime.m_lastRobotTime = 0;
+        robotTime.m_frameNumber = 0;
+        robotTime.endMatchTime();
+        robotTime.m_lastMatchTime = 0;
+
+        // TEST 1
+        assertEquals(0, robotTime.m_deltaTime);
+        assertEquals(0, robotTime.m_robotTime);
+        assertEquals(0, robotTime.m_lastRobotTime);
+        assertEquals(0, robotTime.m_frameNumber);
+        lastTime = robotTime.m_robotTime;
+
+        // TEST 2
+        wait(1);
+        robotTime.updateTimes();
+        assertEquals(true, robotTime.m_deltaTime > 0);
+        assertEquals(true, robotTime.m_robotTime > 0);
+        assertEquals(lastTime, robotTime.m_lastRobotTime);
+        assertEquals(1, robotTime.m_frameNumber);
+        lastTime = robotTime.m_robotTime;
+
+        // TEST 3
+        wait(1);
+        robotTime.updateTimes();
+        assertEquals(true, robotTime.m_deltaTime > 0);
+        assertEquals(true, robotTime.m_robotTime > 0);
+        assertEquals(lastTime, robotTime.m_lastRobotTime);
+        assertEquals(2, robotTime.m_frameNumber);
+    }
+
+    @Test
+    public void testMatchTime() {
+        // SETUP
+        long lastTime;
+
+        // TEST 1
+        assertEquals(0, robotTime.m_matchTime);
+        assertEquals(0, robotTime.m_lastMatchTime);
+        lastTime = robotTime.m_matchTime;
+
+        // TEST 2
+        robotTime.startMatchTime();
+        wait(1);
+        robotTime.updateTimes();
+        assertEquals(true, robotTime.m_matchTime > 0);
+        assertEquals(lastTime, robotTime.m_lastMatchTime);
+        lastTime = robotTime.m_matchTime;
+        
+        // TEST 3
+        wait(1);
+        robotTime.updateTimes();
+        robotTime.endMatchTime();
+        assertEquals(0, robotTime.m_matchTime);
+        assertEquals(lastTime, robotTime.m_lastMatchTime);
+        lastTime = robotTime.m_matchTime;
+
+        // TEST 4
+        wait(1);
+        robotTime.updateTimes();
+        assertEquals(0, robotTime.m_matchTime);
+        assertEquals(lastTime, robotTime.m_lastMatchTime);
+    }
+
+    private void wait(int millis) {
+        try {
+            Thread.sleep(millis);
+        } catch (Exception e) {}
+    }
+}