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);