From f588f812936fffd76f19bd532baa6f7865a63b5a Mon Sep 17 00:00:00 2001 From: Kaya <95276965+kytpbs@users.noreply.github.com> Date: Thu, 20 Jun 2024 01:29:52 +0300 Subject: [PATCH] Create new command tests and add extra fixes (#14) * Test Default Drive command * simplfy drive tests * make rotation boosted as well in `drivewithextras` Also moves boost by to a seperate variable which explains a lot more imo * turn `boostby` to `boostMultiplier` and simplify calculation simplified by using a calculator, so it has a very low chance of being wrong yey! * update actions to run on multiple OS's * builds on mac fail, and we don't target it anyways... I wanted to support mac too but half of the tests somehow fail, so without a mac on my hands I won't support it * update to use .runEnd instead of run this should fix a lot of the auto issues. * make the green blink a part of the parallel race group * Update path and autos * pull master (#13) * update to use .runEnd instead of run this should fix a lot of the auto issues. * make the green blink a part of the parallel race group * Update path and autos * [Dependency] bump wpilib to 2024.3.2 * rename "SmartShootCommand" to "ShootToSpeakerCommand" This represents what it is about a lot better in my opinion. * Fix tests by not shutting down the hal in teardown * close the subsystem after super in basicIntakeTest * add more comments for explanation * don't shutdown HAL in DriveSubsystem test base * remove relaince on sparkMAX for testing this abstracts the motor controller type away from the tests, so it is easier to change the motor controller type while the tests still work correctly. * add helper functions color sensor. setNoteColor abstracts away the set values on the color setting for note so if they need to be changed, its only on 1 place * Create LoadToShooterTest.java --- .github/workflows/build-and-test.yml | 5 +- .../frc/robot/subsystems/DriveSubsystem.java | 7 +- .../frc/robot/subsystems/IntakeSubsystem.java | 8 + .../utils/sim_utils/ColorSensorV3Wrapped.java | 8 + .../command_tests/LEDIdleCommandTest.java | 4 +- .../arm_tests/BasicIntakeTest.java | 11 +- .../arm_tests/LoadToShooterTest.java | 104 +++++++++++++ .../drive_tests/DefaultDriveCommandTest.java | 144 ++++++++++++++++++ .../arm_tests/IntakeTests.java | 44 +++--- .../drive_subsystem_tests/DriveTests.java | 4 +- 10 files changed, 306 insertions(+), 33 deletions(-) create mode 100644 src/test/java/command_tests/arm_tests/LoadToShooterTest.java create mode 100644 src/test/java/command_tests/drive_tests/DefaultDriveCommandTest.java diff --git a/.github/workflows/build-and-test.yml b/.github/workflows/build-and-test.yml index 1e1f5dd..c4a0b12 100644 --- a/.github/workflows/build-and-test.yml +++ b/.github/workflows/build-and-test.yml @@ -6,8 +6,11 @@ on : [push, pull_request] jobs: # This workflow contains a single job called "build" build: + strategy: + matrix: + os: [ubuntu-latest, windows-latest] # The type of runner that the job will run on - runs-on: windows-latest + runs-on: ${{ matrix.os }} # Steps represent a sequence of tasks that will be executed as part of the job steps: diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index e60c995..d762b4a 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DriveSubsystem.java @@ -488,10 +488,11 @@ public void driveWithExtras( boolean fieldRelative, boolean rateLimit) { final double sens = OIConstants.kDriveSensitivity; // The rest will be added by "boost" + final var boostMultiplier = sens * (1 - boost) + boost; drive( - MathUtil.applyDeadband(xSpeed * (sens + (boost * (1 - sens))), deadband), - MathUtil.applyDeadband(ySpeed * (sens + (boost * (1 - sens))), deadband), - MathUtil.applyDeadband(rot * sens, deadband), + MathUtil.applyDeadband(xSpeed * boostMultiplier, deadband), + MathUtil.applyDeadband(ySpeed * boostMultiplier, deadband), + MathUtil.applyDeadband(rot * boostMultiplier, deadband), fieldRelative, rateLimit); SmartDashboard.putNumber("xSpeed: ", xSpeed); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 8af2590..3c83c1e 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -82,6 +82,14 @@ public boolean hasNote() { && red > colorSensor.getGreen(); } + public double getGroundIntakeSpeed() { + return groundIntake.get(); + } + + public double getArmIntakeSpeed() { + return armIntake.get(); + } + public void setIntakeSpeed(double armSpeed, double groundSpeed, boolean force) { if (armSpeed > 0 && hasNote() && !force) { // If we are intaking, check if we have a note. armIntake.set(0); diff --git a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java index 37bcfa1..3b7a723 100644 --- a/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java +++ b/src/main/java/frc/utils/sim_utils/ColorSensorV3Wrapped.java @@ -53,6 +53,14 @@ public static void setRGBD(int red, int green, int blue, int distance) { rgbd[3] = distance; } + public static void setNoteColor(boolean isNote) { + if (isNote) { + setRGBD(1000, 300, 0, 1500); + } else { + setRGBD(0, 0, 0, 0); + } + } + @Override public int getRed() { // we check if getRed() is 0 because the color sensor returns 0 when its not being "simulated" diff --git a/src/test/java/command_tests/LEDIdleCommandTest.java b/src/test/java/command_tests/LEDIdleCommandTest.java index 754da1f..127be99 100644 --- a/src/test/java/command_tests/LEDIdleCommandTest.java +++ b/src/test/java/command_tests/LEDIdleCommandTest.java @@ -51,13 +51,13 @@ void colorOnEnabled() { new LEDIdleCommand(ledSubsystem, intakeSubsystem).ignoringDisable(true)); // * check without Note - ColorSensorV3Wrapped.setRGBD(0, 0, 0, 0); + ColorSensorV3Wrapped.setNoteColor(false); commandScheduler.run(); Timer.delay(0.1); // let led thread do its thing checkForColorInAll(ledSubsystem, Color.kRed, "Color should be red when no Note is detected"); // * check with Note - ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900); + ColorSensorV3Wrapped.setNoteColor(true); commandScheduler.run(); Timer.delay(0.1); // let led thread do its thing checkForColorInAll(ledSubsystem, Color.kGreen, "Color should be green when Note is detected"); diff --git a/src/test/java/command_tests/arm_tests/BasicIntakeTest.java b/src/test/java/command_tests/arm_tests/BasicIntakeTest.java index 9a0e5e4..9024865 100644 --- a/src/test/java/command_tests/arm_tests/BasicIntakeTest.java +++ b/src/test/java/command_tests/arm_tests/BasicIntakeTest.java @@ -4,15 +4,12 @@ import static org.junit.jupiter.api.Assertions.assertNotEquals; import static subsystem_tests.led_tests.utils.LEDTestUtils.checkForColorInAll; -import com.revrobotics.CANSparkMax; import command_tests.utils.CommandTestBase; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; -import frc.robot.Constants.IntakeConstants; import frc.robot.commands.BasicIntakeCommand; import frc.robot.subsystems.IntakeSubsystem; import frc.utils.sim_utils.ColorSensorV3Wrapped; -import frc.utils.sim_utils.SparkMAXSimAddon; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; @@ -20,14 +17,12 @@ class BasicIntakeTest extends CommandTestBase { private IntakeSubsystem intakeSubsystem; - private CANSparkMax intakeMotor; private BasicIntakeCommand intakeCommand; @BeforeEach public void setUp() { super.setUp(); intakeSubsystem = new IntakeSubsystem(); - intakeMotor = SparkMAXSimAddon.getSparkMAX(IntakeConstants.kArmIntakeMotorCanID); intakeCommand = new BasicIntakeCommand(intakeSubsystem); commandScheduler.schedule(intakeCommand); @@ -43,7 +38,7 @@ public void tearDown() { void testSpeed() { commandScheduler.run(); // Motor shouldn't be 0 when intaking - assertNotEquals(0, intakeMotor.get()); + assertNotEquals(0, intakeSubsystem.getArmIntakeSpeed()); } @Test @@ -58,9 +53,9 @@ void testLEDBlinking() { @Test void testCommandEnds() { commandScheduler.run(); - ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900); + ColorSensorV3Wrapped.setNoteColor(true); commandScheduler.run(); assertEquals(true, intakeCommand.isFinished(), "Command should be finished when ending"); - assertEquals(0, intakeMotor.get(), "Motor should stop after detecting color"); + assertEquals(0, intakeSubsystem.getArmIntakeSpeed(), "Motor should stop after detecting color"); } } diff --git a/src/test/java/command_tests/arm_tests/LoadToShooterTest.java b/src/test/java/command_tests/arm_tests/LoadToShooterTest.java new file mode 100644 index 0000000..2153879 --- /dev/null +++ b/src/test/java/command_tests/arm_tests/LoadToShooterTest.java @@ -0,0 +1,104 @@ +package command_tests.arm_tests; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; + +import command_tests.utils.CommandTestBase; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import frc.robot.commands.LoadToShooterCommand; +import frc.robot.subsystems.IntakeSubsystem; +import frc.utils.sim_utils.ColorSensorV3Wrapped; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +class LoadToShooterTest extends CommandTestBase { + IntakeSubsystem intakeSubsystem; + LoadToShooterCommand command; + + @BeforeEach + public void setUp() { + super.setUp(); + + intakeSubsystem = new IntakeSubsystem(); + command = new LoadToShooterCommand(intakeSubsystem); + + commandScheduler.schedule(command); + } + + @AfterEach + public void tearDown() { + super.tearDown(); + intakeSubsystem.close(); + } + + @Test + void testPushesOutNoteAndStops() { + ColorSensorV3Wrapped.setNoteColor(true); + commandScheduler.run(); + SimHooks.stepTiming(0.6); // handle "waitANDCondition" s wait part + commandScheduler.run(); + + ColorSensorV3Wrapped.setNoteColor(false); + commandScheduler.run(); + commandScheduler.run(); + + assertEquals( + false, + command.isScheduled(), + "command should finish after note isn't detected AND 0.5 seconds elapses"); + } + + @Test + void testItDoesntStopBeforeTime() { + ColorSensorV3Wrapped.setNoteColor(true); + commandScheduler.run(); + + SimHooks.stepTiming(0.1); // not enough time for it to stop + commandScheduler.run(); + + ColorSensorV3Wrapped.setNoteColor(false); + commandScheduler.run(); + commandScheduler.run(); + + assertEquals(true, command.isScheduled(), "command shouldn't finish before the time"); + } + + /** This is for the fact that, if somehow the color sensor fails, that might cause issues. */ + @Test + void testItRunsWithoutDetection() { + ColorSensorV3Wrapped.setNoteColor(false); + commandScheduler.run(); + commandScheduler.run(); + + assertEquals( + true, + command.isScheduled(), // + "command should run even if no note cannot be detected"); + + SimHooks.stepTiming(0.6); + + commandScheduler.run(); + commandScheduler.run(); + + assertEquals( + false, + command.isScheduled(), + "command should stop after 0.5 second time, when no note is detected"); + } + + @Test + void testItActuallyRuns() { + ColorSensorV3Wrapped.setNoteColor(false); + commandScheduler.run(); + commandScheduler.run(); + + assertNotEquals(0, intakeSubsystem.getArmIntakeSpeed()); + + SimHooks.stepTiming(0.6); + commandScheduler.run(); + commandScheduler.run(); + + assertEquals(0, intakeSubsystem.getArmIntakeSpeed()); + } +} diff --git a/src/test/java/command_tests/drive_tests/DefaultDriveCommandTest.java b/src/test/java/command_tests/drive_tests/DefaultDriveCommandTest.java new file mode 100644 index 0000000..7d0403f --- /dev/null +++ b/src/test/java/command_tests/drive_tests/DefaultDriveCommandTest.java @@ -0,0 +1,144 @@ +package command_tests.drive_tests; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import command_tests.utils.CommandTestBase; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; +import edu.wpi.first.wpilibj.simulation.SimHooks; +import edu.wpi.first.wpilibj.simulation.XboxControllerSim; +import frc.robot.Constants.DriveConstants; +import frc.robot.Constants.OIConstants; +import frc.robot.commands.DefaultDriveCommand; +import frc.robot.subsystems.DriveSubsystem; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import subsystem_tests.drive_subsystem_tests.utils.DriveTestUtils; + +class DefaultDriveCommandTest extends CommandTestBase { + DriveSubsystem driveSubsystem; + XboxController controller; + XboxControllerSim controllerSim; + DefaultDriveCommand defaultDriveCommand; + + @BeforeEach + public void setUp() { + super.setUp(); + driveSubsystem = new DriveSubsystem(); + controller = new XboxController(0); + controllerSim = new XboxControllerSim(controller); + defaultDriveCommand = new DefaultDriveCommand(driveSubsystem, controller); + commandScheduler.schedule(defaultDriveCommand); + } + + @AfterEach + public void tearDown() { + driveSubsystem.close(); + resetController(); + super.tearDown(); + } + + /** Sets all axis to 0 */ + void resetController() { + for (int i = 0; i < controller.getAxisCount(); i++) { + controllerSim.setRawAxis(i, 0); + } + } + + @Test + void testDefaultDriveCommandForward() { + commandScheduler.run(); + + assertTrue(commandScheduler.isScheduled(defaultDriveCommand)); + + controllerSim.setLeftY(1); + DriverStationSim.notifyNewData(); + assertEquals(1, controller.getLeftY(), 0.001); + commandScheduler.run(); + SimHooks.stepTiming(2); // ! This is for the rate limiter not to kick in + commandScheduler.run(); + + for (var state : driveSubsystem.getModuleDesiredStates()) { + // boost is 0 so speed should be timed with sensitivity + assertEquals( + DriveConstants.kMaxSpeedMetersPerSecond * OIConstants.kDriveSensitivity, + state.speedMetersPerSecond, + 0.05); + assertEquals(Rotation2d.fromDegrees(0), state.angle); + } + } + + @Test + void testDefaultDriveCommandSideways() { + commandScheduler.run(); + + assertTrue(commandScheduler.isScheduled(defaultDriveCommand)); + + controllerSim.setLeftX(1); + DriverStationSim.notifyNewData(); + assertEquals(1, controller.getLeftX(), 0.001); + commandScheduler.run(); + SimHooks.stepTiming(2); // ! This is for the rate limiter not to kick in + commandScheduler.run(); + + for (var state : driveSubsystem.getModuleDesiredStates()) { + // boost is 0 so speed should be timed with sensitivity + assertEquals( + DriveConstants.kMaxSpeedMetersPerSecond * OIConstants.kDriveSensitivity, + state.speedMetersPerSecond, + 0.05); + // -90 because +y is inverted in the field coordinate system + assertEquals(Rotation2d.fromDegrees(-90), state.angle); + } + } + + @Test + void testDefaultDriveCommandBoost() { + commandScheduler.run(); + + assertTrue(commandScheduler.isScheduled(defaultDriveCommand)); + + controllerSim.setLeftY(1); + controllerSim.setRightTriggerAxis(1); + DriverStationSim.notifyNewData(); + assertEquals(1, controller.getLeftY(), 0.001); + commandScheduler.run(); + SimHooks.stepTiming(2); // ! This is for the rate limiter not to kick in + commandScheduler.run(); + + for (var state : driveSubsystem.getModuleDesiredStates()) { + // boost is 1 so speed should be max speed + assertEquals(DriveConstants.kMaxSpeedMetersPerSecond, state.speedMetersPerSecond, 0.05); + assertEquals(Rotation2d.fromDegrees(0), state.angle); + } + } + + @Test + void testDefaultDriveCommandRotate() { + commandScheduler.run(); + + assertTrue(commandScheduler.isScheduled(defaultDriveCommand)); + + controllerSim.setRightX(1); + controllerSim.setRightTriggerAxis(0); + DriverStationSim.notifyNewData(); + assertEquals(1, controller.getRightX(), 0.001); + commandScheduler.run(); + SimHooks.stepTiming(2); // ! This is for the rate limiter not to kick in + commandScheduler.run(); + + // boost is 0 so speed should be timed with sensitivity + // deadband is 0.02 which increases all values by 0.02 + // ! +rot is counter clockwise in field coordinate system, so it should be negative + var shouldBe = + DriveTestUtils.driveToChassisSpeeds( + 0, 0, MathUtil.applyDeadband(-1 * OIConstants.kDriveSensitivity, 0.02)); + var currentlyIs = DriveTestUtils.getDesiredChassisSpeeds(driveSubsystem); + + DriveTestUtils.checkIfEqual(shouldBe, currentlyIs); + } +} diff --git a/src/test/java/subsystem_tests/arm_tests/IntakeTests.java b/src/test/java/subsystem_tests/arm_tests/IntakeTests.java index 10714e0..04f2597 100644 --- a/src/test/java/subsystem_tests/arm_tests/IntakeTests.java +++ b/src/test/java/subsystem_tests/arm_tests/IntakeTests.java @@ -2,26 +2,21 @@ import static org.junit.jupiter.api.Assertions.assertEquals; -import com.revrobotics.CANSparkMax; import edu.wpi.first.hal.HAL; import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants.IntakeConstants; import frc.robot.subsystems.IntakeSubsystem; import frc.utils.sim_utils.ColorSensorV3Wrapped; -import frc.utils.sim_utils.SparkMAXSimAddon; import org.junit.jupiter.api.AfterEach; import org.junit.jupiter.api.BeforeEach; import org.junit.jupiter.api.Test; class IntakeTests { private IntakeSubsystem intakeSubsystem; - private CANSparkMax intakeMotor; @BeforeEach public void setUp() { HAL.initialize(500, 0); intakeSubsystem = new IntakeSubsystem(); - intakeMotor = SparkMAXSimAddon.getSparkMAX(IntakeConstants.kArmIntakeMotorCanID); } @AfterEach @@ -32,46 +27,61 @@ public void tearDown() { @Test void testIntakeSubsystem() { intakeSubsystem.setIntakeSpeed(0.5); - assertEquals(0.5, intakeMotor.get(), 0.001); + assertEquals(0.5, intakeSubsystem.getArmIntakeSpeed(), 0.001); } @Test void testIntakeNoteDetected() { - ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900); + ColorSensorV3Wrapped.setNoteColor(true); assertEquals(true, intakeSubsystem.hasNote(), "Intake should detect a note"); - ColorSensorV3Wrapped.setRGBD(0, 0, 0, 0); + ColorSensorV3Wrapped.setNoteColor(false); assertEquals(false, intakeSubsystem.hasNote(), "Intake should not detect a note"); // TODO: Add more tests for different RGBD values } @Test void testIntakeStopsOnNote() { - ColorSensorV3Wrapped.setRGBD(0, 0, 0, 0); + ColorSensorV3Wrapped.setNoteColor(false); intakeSubsystem.setIntakeSpeed(0.5); assertEquals( 0.5, - intakeMotor.get(), + intakeSubsystem.getArmIntakeSpeed(), 0.001, "Intake motor should be running when a note is not detected"); - ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900); + ColorSensorV3Wrapped.setNoteColor(true); Timer.delay(0.1); // wait until note is detected - assertEquals(0, intakeMotor.get(), 0.001, "Intake motor should stop when a note is detected"); + assertEquals( + 0, + intakeSubsystem.getArmIntakeSpeed(), + 0.001, + "Intake motor should stop when a note is detected"); } @Test void testIntakeForcePushes() { - ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900); + ColorSensorV3Wrapped.setNoteColor(true); intakeSubsystem.setIntakeSpeed(0.5, 0, true); // force push note out - assertEquals(0.5, intakeMotor.get(), 0.001, "Intake Motor Should Run when force pushed"); + assertEquals( + 0.5, + intakeSubsystem.getArmIntakeSpeed(), + 0.001, + "Intake Motor Should Run when force pushed"); Timer.delay(0.15); // if broken, increase this - assertEquals(0.5, intakeMotor.get(), 0.001, "Intake motor should continue to run when forced"); + assertEquals( + 0.5, + intakeSubsystem.getArmIntakeSpeed(), + 0.001, + "Intake motor should continue to run when forced"); } @Test void testIntakeSubsystemWithNote() { - ColorSensorV3Wrapped.setRGBD(2500, 0, 0, 900); + ColorSensorV3Wrapped.setNoteColor(true); intakeSubsystem.setIntakeSpeed(0.5); assertEquals( - 0, intakeMotor.get(), 0.001, "Intake motor should not be running when a note is detected"); + 0, + intakeSubsystem.getArmIntakeSpeed(), + 0.001, + "Intake motor should not be running when a note is detected"); } } diff --git a/src/test/java/subsystem_tests/drive_subsystem_tests/DriveTests.java b/src/test/java/subsystem_tests/drive_subsystem_tests/DriveTests.java index 9eae5c0..c3b0f01 100644 --- a/src/test/java/subsystem_tests/drive_subsystem_tests/DriveTests.java +++ b/src/test/java/subsystem_tests/drive_subsystem_tests/DriveTests.java @@ -33,9 +33,9 @@ void testRotation() { @Test void testDriveChassisSpeed() { - driveSubsystem.driveRobotRelative(new ChassisSpeeds(2, 2, 0)); - var shouldBe = new ChassisSpeeds(2, 2, 0); + driveSubsystem.driveRobotRelative(shouldBe); + var currentlyIs = DriveTestUtils.getDesiredChassisSpeeds(driveSubsystem); DriveTestUtils.checkIfEqual(shouldBe, currentlyIs);