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

@@ -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:

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

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

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"

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

src/test/java/command_tests/arm_tests/BasicIntakeTest.java

@@ -4,30 +4,25 @@
 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;
 
 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");
   }
 }

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());
+  }
+}

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