mirror of
https://github.com/Team4388/Robot-Essentials.git
synced 2026-06-08 16:28:02 -06:00
Merge pull request #17 from Team4388/dependency-injection
Setup Unit Testing
This commit is contained in:
Keenan D. Buckley
@@ -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
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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();
|
||||
}
|
||||
|
||||
@@ -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 {
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
@@ -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) {}
|
||||
}
|
||||
}
|
||||
@@ -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) {}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user