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/Robot.java b/src/main/java/frc4388/robot/Robot.java
index d2c3a6c..e145b38 100644
--- a/src/main/java/frc4388/robot/Robot.java
+++ b/src/main/java/frc4388/robot/Robot.java
@@ -21,7 +21,8 @@ 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();
}
/**
diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java
index 1aaaddc..b7542bc 100644
--- a/src/main/java/frc4388/robot/RobotContainer.java
+++ b/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 LED m_robotLED = new LED();
+ private final Drive m_robotDrive = new Drive(m_robotMap.leftFrontMotor, m_robotMap.rightFrontMotor,
+ 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(
- getDriverController().getLeftYAxis(),
- getDriverController().getRightXAxis()), m_robotDrive));
+ m_robotDrive.setDefaultCommand(
+ new RunCommand(() -> m_robotDrive.driveWithInput(getDriverController().getLeftYAxis(),
+ 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
- * instantiating a {@link GenericHID} or one of its subclasses ({@link
- * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
- * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
- */
+ * Use this method to define your button->command mappings. Buttons can be
+ * created by instantiating a {@link GenericHID} or one of its subclasses
+ * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
+ * 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))
- .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
+ .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
+ .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
}
/**
@@ -85,28 +90,25 @@ public class RobotContainer {
public IHandController getDriverController() {
return m_driverXbox;
}
-
+
/**
* 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();
}
}
diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java
new file mode 100644
index 0000000..7efbfda
--- /dev/null
+++ b/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);
+ }
+}
diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java
index 6252eba..1703513 100644
--- a/src/main/java/frc4388/robot/subsystems/Drive.java
+++ b/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 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_rightBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
- private DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
- private RobotGyro m_gyro = new RobotGyro(new PigeonIMU(DriveConstants.DRIVE_PIGEON_ID));
+ private RobotTime m_robotTime = RobotTime.getInstance();
+
+ private WPI_TalonSRX m_leftFrontMotor;
+ private WPI_TalonSRX m_rightFrontMotor;
+ private WPI_TalonSRX m_leftBackMotor;
+ private WPI_TalonSRX m_rightBackMotor;
+ private DifferentialDrive m_driveTrain;
+ private RobotGyro m_gyro;
/**
* Add your docs here.
*/
- public Drive(){
- /* factory default values */
- m_leftFrontMotor.configFactoryDefault();
- m_rightFrontMotor.configFactoryDefault();
- m_leftBackMotor.configFactoryDefault();
- m_rightBackMotor.configFactoryDefault();
+ public Drive(WPI_TalonSRX leftFrontMotor, WPI_TalonSRX rightFrontMotor, WPI_TalonSRX leftBackMotor,
+ WPI_TalonSRX rightBackMotor, DifferentialDrive driveTrain, RobotGyro gyro) {
- /* set back motors as followers */
- m_leftBackMotor.follow(m_leftFrontMotor);
- m_rightBackMotor.follow(m_rightFrontMotor);
-
- /* set neutral mode */
- m_leftFrontMotor.setNeutralMode(NeutralMode.Brake);
- 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);
+ m_leftFrontMotor = leftFrontMotor;
+ m_rightFrontMotor = rightFrontMotor;
+ m_leftBackMotor = leftBackMotor;
+ m_rightBackMotor = rightBackMotor;
+ m_driveTrain = driveTrain;
+ m_gyro = gyro;
}
@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.putNumber("Turn Rate", getRobotGyro().getRate());
- SmartDashboard.putNumber("Gyro Pitch", getRobotGyro().getPitch());
- SmartDashboard.putData(getRobotGyro());
+ SmartDashboard.putBoolean("Is Gyro a Pigeon?", m_gyro.m_isGyroAPigeon);
+ SmartDashboard.putNumber("Turn Rate", m_gyro.getRate());
+ SmartDashboard.putNumber("Gyro Pitch", m_gyro.getPitch());
+ SmartDashboard.putData(m_gyro);
}
}
diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java
index a2a6f1e..57c7b58 100644
--- a/src/main/java/frc4388/robot/subsystems/LED.java
+++ b/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 Spark LEDController;
+ private LEDPatterns m_currentPattern;
+ private Spark m_LEDController;
/**
* Add your docs here.
*/
- public LED(){
- LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+ public LED(Spark LEDController){
+ 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();
- LEDController.set(currentLED);
+ m_currentPattern = pattern;
+ m_LEDController.set(m_currentPattern.getValue());
}
- @Override
- public void periodic(){
- SmartDashboard.putNumber("LED", currentLED);
+ /**
+ * Add your docs here.
+ * @return
+ */
+ public LEDPatterns getPattern() {
+ return m_currentPattern;
}
}
\ No newline at end of file
diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java
index 76e1831..a42e8c8 100644
--- a/src/main/java/frc4388/utility/RobotGyro.java
+++ b/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 AHRS m_navX;
+ private RobotTime m_robotTime = RobotTime.getInstance();
+
+ 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();
}
diff --git a/src/main/java/frc4388/utility/RobotTime.java b/src/main/java/frc4388/utility/RobotTime.java
index 960366b..694f850 100644
--- a/src/main/java/frc4388/utility/RobotTime.java
+++ b/src/main/java/frc4388/utility/RobotTime.java
@@ -11,29 +11,42 @@ package frc4388.utility;
* Keeps track of Robot times like time passed, delta time, etc
*
All times are in milliseconds
*/
-public final class RobotTime {
- private static long m_currTime = System.currentTimeMillis();
- public static long m_deltaTime = 0;
+public class RobotTime {
+ private long m_currTime = System.currentTimeMillis();
+ public long m_deltaTime = 0;
- private static long m_startRobotTime = m_currTime;
- public static long m_robotTime = 0;
- public static long m_lastRobotTime = 0;
+ private long m_startRobotTime = m_currTime;
+ public long m_robotTime = 0;
+ public long m_lastRobotTime = 0;
- private static long m_startMatchTime = 0;
- public static long m_matchTime = 0;
- public static long m_lastMatchTime = 0;
+ private long m_startMatchTime = 0;
+ public long m_matchTime = 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() {
- if (m_matchTime == 0) {
+ public void startMatchTime() {
+ 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;
}
diff --git a/src/test/java/frc4388/mocks/MockPigeonIMU.java b/src/test/java/frc4388/mocks/MockPigeonIMU.java
new file mode 100644
index 0000000..ecddde7
--- /dev/null
+++ b/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;
+ }
+}
diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java
new file mode 100644
index 0000000..3852edb
--- /dev/null
+++ b/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);
+ }
+}
diff --git a/src/test/java/frc4388/utility/RobotGyroUtilityTest.java b/src/test/java/frc4388/utility/RobotGyroUtilityTest.java
new file mode 100644
index 0000000..7b5d5bb
--- /dev/null
+++ b/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) {}
+ }
+}
diff --git a/src/test/java/frc4388/utility/RobotTimeUtilityTest.java b/src/test/java/frc4388/utility/RobotTimeUtilityTest.java
new file mode 100644
index 0000000..c520321
--- /dev/null
+++ b/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) {}
+ }
+}