From 850f5038893d2867154338f91f0caf1c85f3ad01 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 28 Mar 2020 21:27:15 -0600 Subject: [PATCH 01/11] Create RobotMap to decouple Subsystems from their hardware. With the hardware objects decoupled from the subsystems, we can then pass mock hardware into these subsystems to run simulations/tests on. --- .../java/frc4388/robot/RobotContainer.java | 48 +++++++------ src/main/java/frc4388/robot/RobotMap.java | 71 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Drive.java | 61 ++++++---------- .../java/frc4388/robot/subsystems/LED.java | 26 +++---- 4 files changed, 131 insertions(+), 75 deletions(-) create mode 100644 src/main/java/frc4388/robot/RobotMap.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1aaaddc..381910c 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.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..befd97a --- /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() { + configureLEDSubsystem(); + configureDriveSubsystem(); + } + + /* LED Subsystem */ + public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); + + void configureLEDSubsystem() { + + } + + /* 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 configureDriveSubsystem() { + + /* 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..22870b0 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,38 +24,25 @@ 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 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, 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 = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor); + m_gyro = gyro; } @Override @@ -73,20 +57,19 @@ 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. + */ 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..c4cc39e 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -20,36 +20,36 @@ import frc4388.utility.LEDPatterns; */ public class LED extends SubsystemBase { - private float currentLED; - private Spark LEDController; + private float m_currentLED; + 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); + m_LEDController.set(m_currentLED); 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_currentLED); + } + /** * Add your docs here. */ public void updateLED(){ - LEDController.set(currentLED); + m_LEDController.set(m_currentLED); } /** * Add your docs here. */ public void setPattern(LEDPatterns pattern){ - currentLED = pattern.getValue(); - LEDController.set(currentLED); - } - - @Override - public void periodic(){ - SmartDashboard.putNumber("LED", currentLED); + m_currentLED = pattern.getValue(); + m_LEDController.set(m_currentLED); } } \ No newline at end of file From e373211f274433300322a51209a5aca29885faaa Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 28 Mar 2020 21:42:08 -0600 Subject: [PATCH 02/11] Convert RobotTime into a singleton --- src/main/java/frc4388/robot/Robot.java | 11 ++--- src/main/java/frc4388/robot/RobotMap.java | 9 ++-- .../java/frc4388/robot/subsystems/Drive.java | 4 +- src/main/java/frc4388/utility/RobotGyro.java | 4 +- src/main/java/frc4388/utility/RobotTime.java | 41 ++++++++++++------- 5 files changed, 43 insertions(+), 26 deletions(-) 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/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index befd97a..952bc2b 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -25,15 +25,14 @@ import frc4388.utility.RobotGyro; public class RobotMap { public RobotMap() { - configureLEDSubsystem(); - configureDriveSubsystem(); + configureLEDMotorControllers(); + configureDriveMotorControllers(); } /* LED Subsystem */ public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); - void configureLEDSubsystem() { - + void configureLEDMotorControllers() { } /* Drive Subsystem */ @@ -44,7 +43,7 @@ public class RobotMap { public final DifferentialDrive driveTrain = new DifferentialDrive(leftFrontMotor, rightFrontMotor); public final RobotGyro gyroDrive = new RobotGyro(new PigeonIMU(DriveConstants.DRIVE_PIGEON_ID)); - void configureDriveSubsystem() { + void configureDriveMotorControllers() { /* factory default values */ leftFrontMotor.configFactoryDefault(); diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 22870b0..b32547c 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -24,6 +24,8 @@ public class Drive extends SubsystemBase { // Put methods for controlling this subsystem // here. Call these from Commands. + private RobotTime m_robotTime = RobotTime.getInstance(); + private WPI_TalonSRX m_leftFrontMotor; private WPI_TalonSRX m_rightFrontMotor; private WPI_TalonSRX m_leftBackMotor; @@ -49,7 +51,7 @@ public class Drive extends SubsystemBase { 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(); } } diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 76e1831..ab66d00 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -18,6 +18,8 @@ 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 RobotTime m_robotTime = RobotTime.getInstance(); + private PigeonIMU m_pigeon; private AHRS m_navX; public boolean m_isGyroAPigeon; //true if pigeon, false if navX @@ -149,7 +151,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..d427fb7 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; @@ -50,7 +63,7 @@ public final class RobotTime { /** * Call this in both the auto and periodic inits */ - public static void startMatchTime() { + public void startMatchTime() { if (m_matchTime == 0) { m_startMatchTime = m_currTime; m_matchTime = 1; @@ -60,7 +73,7 @@ public final class RobotTime { /** * Call this in disabled init */ - public static void endMatchTime() { + public void endMatchTime() { m_startMatchTime = 0; m_matchTime = 0; } From 489d4a5bff450f5053283d294baf62051a538a92 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 28 Mar 2020 22:45:44 -0600 Subject: [PATCH 03/11] 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); + } +} From 57442554ad029956fbd53a30ad72a942457f48bf Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 28 Mar 2020 22:47:55 -0600 Subject: [PATCH 04/11] Broken test for testing the CI --- src/test/java/frc4388/robot/utility/GyroHeadingTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/test/java/frc4388/robot/utility/GyroHeadingTest.java b/src/test/java/frc4388/robot/utility/GyroHeadingTest.java index 396a615..69827a1 100644 --- a/src/test/java/frc4388/robot/utility/GyroHeadingTest.java +++ b/src/test/java/frc4388/robot/utility/GyroHeadingTest.java @@ -32,7 +32,7 @@ public class GyroHeadingTest { 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(0, gyro.getHeading(1), 0.0001); assertEquals(180, gyro.getHeading(180), 0.0001); assertEquals(-180, gyro.getHeading(-180), 0.0001); assertEquals(97, gyro.getHeading(1537), 0.0001); From 9be0a7fe483b51fb99bc6b5d33c5c9d575a27340 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 28 Mar 2020 22:49:32 -0600 Subject: [PATCH 05/11] Fix broken test --- src/test/java/frc4388/robot/utility/GyroHeadingTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/test/java/frc4388/robot/utility/GyroHeadingTest.java b/src/test/java/frc4388/robot/utility/GyroHeadingTest.java index 69827a1..396a615 100644 --- a/src/test/java/frc4388/robot/utility/GyroHeadingTest.java +++ b/src/test/java/frc4388/robot/utility/GyroHeadingTest.java @@ -32,7 +32,7 @@ public class GyroHeadingTest { 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(1), 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); From 7cad919e7143edb9aa6f7c7befd1206c69f21c5f Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 28 Mar 2020 23:15:45 -0600 Subject: [PATCH 06/11] Improve Gyro Heading Test --- .../RobotGyroUtilityTest.java} | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) rename src/test/java/frc4388/{robot/utility/GyroHeadingTest.java => utility/RobotGyroUtilityTest.java} (87%) diff --git a/src/test/java/frc4388/robot/utility/GyroHeadingTest.java b/src/test/java/frc4388/utility/RobotGyroUtilityTest.java similarity index 87% rename from src/test/java/frc4388/robot/utility/GyroHeadingTest.java rename to src/test/java/frc4388/utility/RobotGyroUtilityTest.java index 396a615..74c589c 100644 --- a/src/test/java/frc4388/robot/utility/GyroHeadingTest.java +++ b/src/test/java/frc4388/utility/RobotGyroUtilityTest.java @@ -5,28 +5,25 @@ /* the project. */ /*----------------------------------------------------------------------------*/ -package frc4388.robot.utility; +package frc4388.utility; import static org.junit.Assert.*; import static org.mockito.Mockito.*; - -import com.ctre.phoenix.sensors.PigeonIMU; +import org.junit.*; import edu.wpi.first.wpilibj.*; +import com.ctre.phoenix.sensors.PigeonIMU; import frc4388.utility.RobotGyro; -import org.junit.*; - /** * Add your docs here. */ -public class GyroHeadingTest { +public class RobotGyroUtilityTest { + // Arrange + RobotGyro gyro = new RobotGyro(mock(PigeonIMU.class)); @Test - public void testConstructor() { - // Arrange - RobotGyro gyro = new RobotGyro(mock(PigeonIMU.class)); - + public void testHeading() { // Act & Assert assertEquals(-90, gyro.getHeading(270), 0.0001); assertEquals(-45, gyro.getHeading(315), 0.0001); From f06b36e443d3c9de23b81a0ba927b2629aebd336 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sat, 28 Mar 2020 23:16:04 -0600 Subject: [PATCH 07/11] Add LEDSubsystem Test --- .../java/frc4388/robot/subsystems/LED.java | 19 +++++--- .../robot/subsystems/LEDSubsystemTest.java | 43 +++++++++++++++++++ 2 files changed, 57 insertions(+), 5 deletions(-) create mode 100644 src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index c4cc39e..a30b1b3 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -21,6 +21,7 @@ import frc4388.utility.LEDPatterns; public class LED extends SubsystemBase { private float m_currentLED; + private LEDPatterns m_currentPattern; private Spark m_LEDController; /** @@ -29,27 +30,35 @@ public class LED extends SubsystemBase { public LED(Spark LEDController){ m_LEDController = LEDController; setPattern(LEDConstants.DEFAULT_PATTERN); - m_LEDController.set(m_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_currentLED); + SmartDashboard.putNumber("LED", m_currentPattern.getValue()); } /** * Add your docs here. */ public void updateLED(){ - m_LEDController.set(m_currentLED); + m_LEDController.set(m_currentPattern.getValue()); } /** * Add your docs here. */ public void setPattern(LEDPatterns pattern){ - m_currentLED = pattern.getValue(); - m_LEDController.set(m_currentLED); + m_currentPattern = pattern; + m_LEDController.set(m_currentPattern.getValue()); + } + + /** + * Add your docs here. + * @return + */ + public LEDPatterns getPattern() { + return m_currentPattern; } } \ No newline at end of file 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..c4ce771 --- /dev/null +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java @@ -0,0 +1,43 @@ +/*----------------------------------------------------------------------------*/ +/* 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() { + assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001); + + led.setPattern(LEDPatterns.RAINBOW_RAINBOW); + assertEquals(LEDPatterns.RAINBOW_RAINBOW.getValue(), led.getPattern().getValue(), 0.0001); + + led.setPattern(LEDPatterns.BLUE_BREATH); + assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001); + + led.setPattern(LEDPatterns.C1_SCANNER); + assertEquals(LEDPatterns.C1_SCANNER.getValue(), led.getPattern().getValue(), 0.0001); + + led.setPattern(LEDPatterns.C1_CHASE); + assertEquals(LEDPatterns.C1_CHASE.getValue(), led.getPattern().getValue(), 0.0001); + + led.setPattern(LEDPatterns.SOLID_BLACK); + assertEquals(LEDPatterns.SOLID_BLACK.getValue(), led.getPattern().getValue(), 0.0001); + } +} From d354ac078bbbd53102bd3e134e159d1e8aaeff87 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sun, 29 Mar 2020 00:10:06 -0600 Subject: [PATCH 08/11] Create Mock PigeonIMU and write more Unit tests - Test to make sure the RoboyGyro class knows what kind of gyro its using - Test to make sure all the getters for the gyro class work (at least for pigeon) --- .../java/frc4388/mocks/MockPigeonIMU.java | 54 ++++++++++++ src/main/java/frc4388/utility/RobotGyro.java | 4 +- .../robot/subsystems/LEDSubsystemTest.java | 6 -- .../frc4388/utility/RobotGyroUtilityTest.java | 83 ++++++++++++++++--- 4 files changed, 128 insertions(+), 19 deletions(-) create mode 100644 src/main/java/frc4388/mocks/MockPigeonIMU.java diff --git a/src/main/java/frc4388/mocks/MockPigeonIMU.java b/src/main/java/frc4388/mocks/MockPigeonIMU.java new file mode 100644 index 0000000..ecddde7 --- /dev/null +++ b/src/main/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/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index 6d5ba5c..d7d1adf 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -20,8 +20,8 @@ import edu.wpi.first.wpiutil.math.MathUtil; public class RobotGyro extends GyroBase { private RobotTime m_robotTime = RobotTime.getInstance(); - private PigeonIMU m_pigeon; - 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; diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java index c4ce771..14c1ce8 100644 --- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java @@ -31,12 +31,6 @@ public class LEDSubsystemTest { led.setPattern(LEDPatterns.BLUE_BREATH); assertEquals(LEDPatterns.BLUE_BREATH.getValue(), led.getPattern().getValue(), 0.0001); - led.setPattern(LEDPatterns.C1_SCANNER); - assertEquals(LEDPatterns.C1_SCANNER.getValue(), led.getPattern().getValue(), 0.0001); - - led.setPattern(LEDPatterns.C1_CHASE); - assertEquals(LEDPatterns.C1_CHASE.getValue(), led.getPattern().getValue(), 0.0001); - 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 index 74c589c..7bcc8c3 100644 --- a/src/test/java/frc4388/utility/RobotGyroUtilityTest.java +++ b/src/test/java/frc4388/utility/RobotGyroUtilityTest.java @@ -13,26 +13,87 @@ 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 { - // Arrange - RobotGyro gyro = new RobotGyro(mock(PigeonIMU.class)); + MockPigeonIMU pigeon = new MockPigeonIMU(DriveConstants.DRIVE_PIGEON_ID); + RobotGyro gyroPigeon = new RobotGyro(pigeon); + AHRS navX = mock(AHRS.class); + RobotGyro gyroNavX = new RobotGyro(navX); + + // TODO UNTESTED: calibrate(), getRate(), and most functions for NavX + + @Test + public void testConfig() { + assertEquals(true, gyroPigeon.m_isGyroAPigeon); + assertEquals(pigeon, gyroPigeon.getPigeon()); + assertEquals(null, gyroPigeon.getNavX()); + assertEquals(false, gyroNavX.m_isGyroAPigeon); + assertEquals(navX, gyroNavX.getNavX()); + assertEquals(null, gyroNavX.getPigeon()); + } @Test public void testHeading() { // 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); + 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() { + assertEquals(0, gyroPigeon.getAngle(), 0.0001); + + pigeon.setYaw(40); + assertEquals(40, gyroPigeon.getAngle(), 0.0001); + + gyroPigeon.reset(); + assertEquals(0, gyroPigeon.getAngle(), 0.0001); + + 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); + + pigeon.setCurrentPitch(45); + pigeon.setCurrentRoll(45); + assertEquals(45, gyroPigeon.getPitch(), 0.0001); + assertEquals(45, gyroPigeon.getRoll(), 0.0001); + + pigeon.setCurrentPitch(0); + pigeon.setCurrentRoll(0); + assertEquals(0, gyroPigeon.getPitch(), 0.0001); + assertEquals(0, gyroPigeon.getRoll(), 0.0001); + + pigeon.setCurrentPitch(-60); + pigeon.setCurrentRoll(-60); + assertEquals(-60, gyroPigeon.getPitch(), 0.0001); + assertEquals(-60, gyroPigeon.getRoll(), 0.0001); + + pigeon.setCurrentPitch(-90); + pigeon.setCurrentRoll(-90); + assertEquals(-90, gyroPigeon.getPitch(), 0.0001); + assertEquals(-90, gyroPigeon.getRoll(), 0.0001); + + pigeon.setCurrentPitch(-100); + pigeon.setCurrentRoll(-100); + assertEquals(-90, gyroPigeon.getPitch(), 0.0001); + assertEquals(-90, gyroPigeon.getRoll(), 0.0001); } } From 21566c6c1529b2ada865228ca74d510ed1b717b5 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Sun, 29 Mar 2020 00:26:57 -0600 Subject: [PATCH 09/11] Cleanup Code --- src/main/java/frc4388/robot/subsystems/LED.java | 1 - src/{main => test}/java/frc4388/mocks/MockPigeonIMU.java | 0 2 files changed, 1 deletion(-) rename src/{main => test}/java/frc4388/mocks/MockPigeonIMU.java (100%) diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index a30b1b3..57c7b58 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -20,7 +20,6 @@ import frc4388.utility.LEDPatterns; */ public class LED extends SubsystemBase { - private float m_currentLED; private LEDPatterns m_currentPattern; private Spark m_LEDController; diff --git a/src/main/java/frc4388/mocks/MockPigeonIMU.java b/src/test/java/frc4388/mocks/MockPigeonIMU.java similarity index 100% rename from src/main/java/frc4388/mocks/MockPigeonIMU.java rename to src/test/java/frc4388/mocks/MockPigeonIMU.java From a56ca65d145766579767f2fbcc6efc7779c7e50e Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Wed, 8 Apr 2020 09:30:16 -0600 Subject: [PATCH 10/11] Add tests for RobotTime class --- src/main/java/frc4388/utility/RobotTime.java | 5 +- .../frc4388/utility/RobotTimeUtilityTest.java | 93 +++++++++++++++++++ 2 files changed, 95 insertions(+), 3 deletions(-) create mode 100644 src/test/java/frc4388/utility/RobotTimeUtilityTest.java diff --git a/src/main/java/frc4388/utility/RobotTime.java b/src/main/java/frc4388/utility/RobotTime.java index d427fb7..694f850 100644 --- a/src/main/java/frc4388/utility/RobotTime.java +++ b/src/main/java/frc4388/utility/RobotTime.java @@ -55,7 +55,7 @@ public class RobotTime { m_deltaTime = m_robotTime - m_lastRobotTime; m_frameNumber++; - if (m_matchTime != 0) { + if (m_startMatchTime != 0) { m_matchTime = m_currTime - m_startMatchTime; } } @@ -64,9 +64,8 @@ public class RobotTime { * Call this in both the auto and periodic inits */ public void startMatchTime() { - if (m_matchTime == 0) { + if (m_startMatchTime == 0) { m_startMatchTime = m_currTime; - m_matchTime = 1; } } diff --git a/src/test/java/frc4388/utility/RobotTimeUtilityTest.java b/src/test/java/frc4388/utility/RobotTimeUtilityTest.java new file mode 100644 index 0000000..96bf5b5 --- /dev/null +++ b/src/test/java/frc4388/utility/RobotTimeUtilityTest.java @@ -0,0 +1,93 @@ +/*----------------------------------------------------------------------------*/ +/* 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() { + long lastTime; + + // Initialisation + assertEquals(0, robotTime.m_deltaTime); + assertEquals(0, robotTime.m_robotTime); + assertEquals(0, robotTime.m_lastRobotTime); + assertEquals(0, robotTime.m_frameNumber); + lastTime = robotTime.m_robotTime; + + wait(1); + robotTime.updateTimes(); + + // First Frame + 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; + + wait(1); + robotTime.updateTimes(); + + // Second Frame + 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() { + long lastTime; + + // Second Frame + assertEquals(0, robotTime.m_matchTime); + assertEquals(0, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; + + robotTime.startMatchTime(); + wait(1); + robotTime.updateTimes(); + + // Third Frame + assertEquals(true, robotTime.m_matchTime > 0); + assertEquals(lastTime, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; + + wait(1); + robotTime.updateTimes(); + robotTime.endMatchTime(); + + // Fourth Frame + assertEquals(0, robotTime.m_matchTime); + assertEquals(lastTime, robotTime.m_lastMatchTime); + lastTime = robotTime.m_matchTime; + + wait(1); + robotTime.updateTimes(); + + // Fifth Frame + assertEquals(0, robotTime.m_matchTime); + assertEquals(lastTime, robotTime.m_lastMatchTime); + } + + private void wait(int millis) { + try { + Thread.sleep(millis); + } catch (Exception e) {} + } +} From 1ec273a39bb1ef6fdb94b4f38be76d0d82872c02 Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Wed, 8 Apr 2020 10:27:33 -0600 Subject: [PATCH 11/11] Add tests for getRate() function in RobotGyro --- src/main/java/frc4388/robot/RobotMap.java | 1 + src/main/java/frc4388/utility/RobotGyro.java | 2 +- .../robot/subsystems/LEDSubsystemTest.java | 4 ++ .../frc4388/utility/RobotGyroUtilityTest.java | 62 ++++++++++++++++++- .../frc4388/utility/RobotTimeUtilityTest.java | 27 ++++---- 5 files changed, 80 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 952bc2b..7efbfda 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -33,6 +33,7 @@ public class RobotMap { public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); void configureLEDMotorControllers() { + } /* Drive Subsystem */ diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index d7d1adf..a42e8c8 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -159,7 +159,7 @@ public class RobotGyro extends GyroBase { @Override public double getRate() { if (m_isGyroAPigeon) { - return m_deltaPigeonAngle / (m_robotTime.m_deltaTime * 1000); + return m_deltaPigeonAngle / m_robotTime.m_deltaTime * 1000; } else { return m_navX.getRate(); } diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java index 14c1ce8..3852edb 100644 --- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java @@ -23,14 +23,18 @@ public class LEDSubsystemTest { @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 index 7bcc8c3..7b5d5bb 100644 --- a/src/test/java/frc4388/utility/RobotGyroUtilityTest.java +++ b/src/test/java/frc4388/utility/RobotGyroUtilityTest.java @@ -27,14 +27,18 @@ public class RobotGyroUtilityTest { RobotGyro gyroPigeon = new RobotGyro(pigeon); AHRS navX = mock(AHRS.class); RobotGyro gyroNavX = new RobotGyro(navX); + RobotTime robotTime = RobotTime.getInstance(); - // TODO UNTESTED: calibrate(), getRate(), and most functions for NavX + // 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()); @@ -42,7 +46,7 @@ public class RobotGyroUtilityTest { @Test public void testHeading() { - // Act & Assert + // TESTS assertEquals(-90, gyroPigeon.getHeading(270), 0.0001); assertEquals(-45, gyroPigeon.getHeading(315), 0.0001); assertEquals(-60, gyroPigeon.getHeading(-60), 0.0001); @@ -55,15 +59,19 @@ public class RobotGyroUtilityTest { } @Test - public void testYawPitchRoll() { + 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); @@ -71,29 +79,77 @@ public class RobotGyroUtilityTest { 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 index 96bf5b5..c520321 100644 --- a/src/test/java/frc4388/utility/RobotTimeUtilityTest.java +++ b/src/test/java/frc4388/utility/RobotTimeUtilityTest.java @@ -21,29 +21,34 @@ public class RobotTimeUtilityTest { @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; - // Initialisation + // 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(); - - // First Frame 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(); - - // Second Frame assertEquals(true, robotTime.m_deltaTime > 0); assertEquals(true, robotTime.m_robotTime > 0); assertEquals(lastTime, robotTime.m_lastRobotTime); @@ -52,35 +57,33 @@ public class RobotTimeUtilityTest { @Test public void testMatchTime() { + // SETUP long lastTime; - // Second Frame + // TEST 1 assertEquals(0, robotTime.m_matchTime); assertEquals(0, robotTime.m_lastMatchTime); lastTime = robotTime.m_matchTime; + // TEST 2 robotTime.startMatchTime(); wait(1); robotTime.updateTimes(); - - // Third Frame assertEquals(true, robotTime.m_matchTime > 0); assertEquals(lastTime, robotTime.m_lastMatchTime); lastTime = robotTime.m_matchTime; + // TEST 3 wait(1); robotTime.updateTimes(); robotTime.endMatchTime(); - - // Fourth Frame assertEquals(0, robotTime.m_matchTime); assertEquals(lastTime, robotTime.m_lastMatchTime); lastTime = robotTime.m_matchTime; + // TEST 4 wait(1); robotTime.updateTimes(); - - // Fifth Frame assertEquals(0, robotTime.m_matchTime); assertEquals(lastTime, robotTime.m_lastMatchTime); }