Merge branch 'master' of https://github.com/MEFThunders-7035/MEF-Thun…

…ders-2024

build.gradle

@@ -1,5 +1,6 @@
 plugins {
     id "java"
+    id "jacoco"
     id "edu.wpi.first.GradleRIO" version "2024.2.1"
 		id "com.diffplug.spotless" version "6.23.3"
 }
@@ -8,7 +9,7 @@ spotless {
   java {
 	    target fileTree('.') {
             include '**/*.java'
-            exclude '**/build/**', '**/build-*/**'
+            exclude '**/build/**', '**/build-*/**', '**/logs/**'
       }
       toggleOffOn()
       googleJavaFormat('1.19.1')
@@ -25,6 +26,19 @@ java {
     targetCompatibility = JavaVersion.VERSION_17
 }
 
+jacoco {
+    toolVersion = "0.8.11"
+    reportsDirectory = layout.projectDirectory.dir("logs/jacoco")
+}
+
+jacocoTestReport {
+    reports {
+        xml.required = false
+        csv.required = false
+        html.outputLocation = layout.projectDirectory.dir("logs/jacocoHtml")
+    }
+}
+
 def ROBOT_MAIN_CLASS = "frc.robot.Main"
 
 // Define my targets (RoboRIO) and artifacts (deployable files)
@@ -91,6 +105,11 @@ dependencies {
 test {
     useJUnitPlatform()
     systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+    finalizedBy jacocoTestReport // report is always generated after tests run
+}
+
+jacocoTestReport {
+    dependsOn test // tests are required to run before generating the report
 }
 
 // Simulation configuration (e.g. environment variables).

src/main/java/frc/robot/RobotContainer.java

@@ -20,7 +20,6 @@
 import frc.robot.Constants.IntakeConstants;
 import frc.robot.commands.ArmIdleCommand;
 import frc.robot.commands.BasicIntakeCommand;
-import frc.robot.commands.ReallySimpleAuto;
 import frc.robot.commands.ShootToAmpCommand;
 import frc.robot.commands.SmartIntakeCommand;
 import frc.robot.commands.SmartShootCommand;
@@ -58,9 +57,6 @@ public RobotContainer() {
         "Shoot To Shooter",
         new SmartShootCommand(shooterSubsystem, intakeSubsystem, armSubsystem, driveSubsystem));
     PhotonCameraSystem.getAprilTagWithID(0); // Load the class before enable.
-    autoChooser.addOption(
-        "Shoot & Back",
-        new ReallySimpleAuto(armSubsystem, intakeSubsystem, shooterSubsystem, driveSubsystem));
     SmartDashboard.putData("Auto Chooser", autoChooser);
     if (RobotBase.isSimulation()) {
       simInit();

src/main/java/frc/robot/commands/BasicRunShooterCommand.java

@@ -1,21 +1,25 @@
 package frc.robot.commands;
 
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import frc.robot.Constants.ShooterConstants;
 import frc.robot.commands.led_commands.LEDLoadingWaitCommand;
 import frc.robot.subsystems.ShooterSubsystem;
 
 public class BasicRunShooterCommand extends SequentialCommandGroup {
-  BasicRunShooterCommand(ShooterSubsystem shooterSubsystem) {
+  public BasicRunShooterCommand(ShooterSubsystem shooterSubsystem) {
     super(
         shooterSubsystem.run(
             () -> shooterSubsystem.setShooterSpeed(ShooterConstants.kShooterSpeed)));
   }
 
-  BasicRunShooterCommand(ShooterSubsystem shooterSubsystem, double untilTimeSeconds) {
+  public BasicRunShooterCommand(ShooterSubsystem shooterSubsystem, double untilTimeSeconds) {
     super(
-        shooterSubsystem
-            .run(() -> shooterSubsystem.setShooterSpeed(ShooterConstants.kShooterSpeed))
-            .raceWith(new LEDLoadingWaitCommand(untilTimeSeconds)));
+        new ParallelRaceGroup(
+            new BasicRunShooterCommand(shooterSubsystem), // Run the shooter
+            new LEDLoadingWaitCommand(untilTimeSeconds)),
+        shooterSubsystem.runOnce(
+            () -> shooterSubsystem.setShooterSpeed(0) // Stop the shooter after the wait
+            ));
   }
 }

src/main/java/frc/robot/commands/MoveArmToAmp.java

@@ -4,7 +4,7 @@
 import frc.robot.subsystems.ArmSubsystem;
 
 public class MoveArmToAmp extends SequentialCommandGroup {
-  MoveArmToAmp(ArmSubsystem armSubsystem) {
+  public MoveArmToAmp(ArmSubsystem armSubsystem) {
     super(
         armSubsystem
             .run(() -> armSubsystem.setArmToPosition(0.5))

src/main/java/frc/robot/commands/VibrateControllerCommand.java

@@ -6,6 +6,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.commands.util_commands.RepeatForCommand;
 
 public class VibrateControllerCommand extends SequentialCommandGroup {
   public VibrateControllerCommand(
@@ -24,16 +25,13 @@ public VibrateControllerCommand(XboxController controller, int repetitions) {
   private static Command createVibrateControllerCommand(
       XboxController controller, int repetitions, double intensity, double duration) {
 
-    Command command =
-        new InstantCommand(() -> controller.setRumble(RumbleType.kBothRumble, intensity));
-    for (int i = 0; i < repetitions; i++) {
-      command =
-          command
-              .andThen(new WaitCommand(duration))
-              .andThen(() -> controller.setRumble(RumbleType.kBothRumble, 0))
-              .andThen(new WaitCommand(duration))
-              .andThen(() -> controller.setRumble(RumbleType.kBothRumble, intensity));
-    }
-    return command.finallyDo(() -> controller.setRumble(RumbleType.kBothRumble, 0));
+    SequentialCommandGroup toRepeatCommand =
+        new SequentialCommandGroup(
+            new InstantCommand(() -> controller.setRumble(RumbleType.kBothRumble, intensity)), //
+            new WaitCommand(duration),
+            new InstantCommand(() -> controller.setRumble(RumbleType.kBothRumble, 0)),
+            new WaitCommand(duration));
+
+    return new RepeatForCommand(toRepeatCommand, repetitions);
   }
 }

src/main/java/frc/robot/commands/led_commands/LEDLoadingWaitCommand.java

@@ -7,6 +7,7 @@
 public class LEDLoadingWaitCommand extends WaitCommand {
   private double seconds;
   private Color colorToFill;
+  public static final Color DEFAULT_COLOR = new Color(0, 200, 255);
 
   public LEDLoadingWaitCommand(double seconds, Color colorToFill) {
     super(seconds);
@@ -16,7 +17,7 @@ public LEDLoadingWaitCommand(double seconds, Color colorToFill) {
   }
 
   public LEDLoadingWaitCommand(double seconds) {
-    this(seconds, new Color(0, 200, 255));
+    this(seconds, DEFAULT_COLOR);
   }
 
   @Override

src/main/java/frc/robot/commands/util_commands/RepeatForCommand.java

@@ -0,0 +1,35 @@
+package frc.robot.commands.util_commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RepeatCommand;
+
+public class RepeatForCommand extends RepeatCommand {
+  private double count;
+  private Command command;
+
+  /**
+   * Repeats a command for a specified number of times
+   *
+   * @param command The command to repeat
+   * @param count The number of times to repeat the command
+   */
+  public RepeatForCommand(Command command, int count) {
+    super(command);
+    this.command = command;
+    this.count = count;
+  }
+
+  @Override
+  public void execute() {
+    // only decrement the count if the command has finished
+    if (command.isFinished()) {
+      count--;
+    }
+    super.execute();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return count <= 0;
+  }
+}

src/main/java/frc/robot/subsystems/ArmSubsystem.java

@@ -16,7 +16,7 @@
 import frc.utils.ExtraFunctions;
 import frc.utils.sim_utils.CANSparkMAXWrapped;
 
-public class ArmSubsystem extends SubsystemBase {
+public class ArmSubsystem extends SubsystemBase implements AutoCloseable {
   // We only have RelativeEncoder for now. Its better than nothing.
   private final CANSparkMAXWrapped arm;
   private final CANSparkMAXWrapped armFollower;
@@ -63,6 +63,12 @@ private void setupSparkMax() {
     arm.burnFlash();
   }
 
+  @Override
+  public void close() {
+    arm.close();
+    armFollower.close();
+  }
+
   /**
    * Sets the arm to a position. This is a wrapper for the Spark Max's set method.
    *

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -20,6 +20,7 @@ public class IntakeSubsystem extends SubsystemBase implements AutoCloseable {
   private final CANSparkMAXWrapped armIntake;
   private final CANSparkMAXWrapped groundIntake;
   private final ColorSensorV3Wrapped colorSensor;
+  private final Thread fastColorCheckThread;
   private boolean isForced = false;
 
   public IntakeSubsystem() {
@@ -29,14 +30,16 @@ public IntakeSubsystem() {
     setupIntakeMotors();
     colorSensor = new ColorSensorV3Wrapped(ColorSensorConstants.kColorSensorPort);
 
-    new Thread(
+    fastColorCheckThread =
+        new Thread(
             () -> {
               while (true) {
                 fastPeriodic();
               }
             },
-            "Fast Color Check Loop")
-        .start();
+            "Fast Color Check Loop");
+    fastColorCheckThread.setDaemon(true);
+    fastColorCheckThread.start();
   }
 
   private void setupIntakeMotors() {
@@ -57,6 +60,7 @@ private void setupIntakeMotors() {
 
   @Override
   public void close() {
+    fastColorCheckThread.interrupt();
     armIntake.close();
     groundIntake.close();
     colorSensor.close();
@@ -127,7 +131,7 @@ public void periodic() {
     SmartDashboard.putNumber("ColorSensor - Blue", colorSensor.getBlue());
     SmartDashboard.putNumber("ColorSensor - IR", colorSensor.getIR());
     SmartDashboard.putBoolean("Note Detected", hasNote());
-    checkIfHasNote();
+    // don't check for a note here cause it breaks the unit tests SOMEHOW!
   }
 
   private void fastPeriodic() {

src/main/java/frc/robot/subsystems/LEDSubsystem.java

@@ -7,7 +7,7 @@
 import frc.robot.Constants.LEDConstants;
 import frc.utils.BetterLED;
 
-public class LEDSubsystem extends SubsystemBase {
+public class LEDSubsystem extends SubsystemBase implements AutoCloseable {
   BetterLED strip;
   Color lastSetColor = new Color();
   Color lastSetBlinkingColor = new Color();
@@ -22,6 +22,15 @@ public void periodic() {
     SmartDashboard.putString("LED Currently Running", strip.getCurrentCommandName());
   }
 
+  @Override
+  public void close() {
+    strip.close();
+  }
+
+  public int getLedCount() {
+    return strip.getLedCount();
+  }
+
   public void fill(Color color) {
     if (lastSetColor.equals(color)) {
       return;
@@ -68,13 +77,17 @@ public void setStatusColor(boolean status) {
     }
   }
 
-  public void blink(Color color) {
+  public void blink(Color color, double delaySeconds) {
     if (lastSetBlinkingColor.equals(color)) {
       return;
     }
     lastSetColor = new Color(); // Reset back to 0
     lastSetBlinkingColor = color;
-    strip.blink(color);
+    strip.blink(color, delaySeconds);
+  }
+
+  public void blink(Color color) {
+    blink(color, 0.2);
   }
 
   public void blinkRed() {
@@ -84,6 +97,12 @@ public void blinkRed() {
     blink(new Color(255, 0, 0));
   }
 
+  /**
+   * Fills the led's until the given percentage.
+   *
+   * @param percentage a value between 0 and 1
+   * @param color the color to fill with
+   */
   public void fillPercentageWithColor(double percentage, Color color) {
     fill(color, (int) (strip.getLedCount() * percentage), true);
   }

src/main/java/frc/robot/subsystems/LEDSystem.java

@@ -3,7 +3,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 
 public final class LEDSystem {
-  private static final LEDSubsystem ledSubsystemInstance = new LEDSubsystem();
+  private static LEDSubsystem ledSubsystemInstance = new LEDSubsystem();
 
   private LEDSystem() {}
 
@@ -18,4 +18,17 @@ public static Command run(Runnable toRun) {
   public static Command getBlinkRedCommand() {
     return ledSubsystemInstance.getRedBlinkCommand();
   }
+
+  public static void resetLEDSubsystem() {
+    if (ledSubsystemInstance == null) {
+      ledSubsystemInstance = new LEDSubsystem();
+      return;
+    }
+    ledSubsystemInstance.close();
+    ledSubsystemInstance = new LEDSubsystem();
+  }
+
+  public static void changeLEDSubsystemInstance(LEDSubsystem ledSubsystem) {
+    ledSubsystemInstance = ledSubsystem;
+  }
 }

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -3,13 +3,18 @@
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class ShooterSubsystem extends SubsystemBase {
+public class ShooterSubsystem extends SubsystemBase implements AutoCloseable {
   private final Spark shooterMotor;
 
   public ShooterSubsystem() {
     shooterMotor = new Spark(0);
   }
 
+  @Override
+  public void close() {
+    shooterMotor.close();
+  }
+
   public void setShooterSpeed(double speed) {
     shooterMotor.set(speed);
   }