Added startup connection checking

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

@@ -30,9 +30,11 @@
 import frc.robot.Constants.ShooterConstants;
 import frc.robot.Constants.SwerveConstants;
 import frc.robot.Constants.VisionConstants;
+import frc.robot.commands.StartupConnectionCheck;
 import frc.robot.commands.TeleopSwerve;
 import frc.robot.commands.arm.ArmHold;
 import frc.robot.commands.arm.AutoShoot;
+import frc.robot.commands.leds.LoadingAnimation;
 import frc.robot.commands.leds.RSLSync;
 import frc.robot.commands.leds.SolidColor;
 import frc.robot.subsystems.Arm;
@@ -128,6 +130,12 @@ public RobotContainer() {
             SwerveConstants.maxTranslationalSpeed,
             SwerveConstants.maxAngularSpeed,
             swerve));
+
+    new StartupConnectionCheck(
+            new LoadingAnimation(Color.kBlue, leds),
+            new SolidColor(Color.kGreen, leds).withTimeout(5.0),
+            new SolidColor(Color.kRed, leds).withTimeout(5.0))
+        .schedule();
   }
 
   /**

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

@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj2.command.Command;
+import java.net.InetAddress;
+import java.util.function.Consumer;
+
+public class RadioPing extends Command {
+  private Notifier notifier;
+
+  private InetAddress ipAddress;
+
+  private boolean receivedConnection = false;
+  private int responseCount = 0;
+
+  boolean initialFailed = false;
+  Consumer<Boolean> onInitialFailed;
+
+  /** Creates a new RadioPing. */
+  public RadioPing(Consumer<Boolean> onInitialFailed) {
+    try {
+      ipAddress = InetAddress.getByName("10.3.53.1");
+    } catch (Exception e) {
+      e.printStackTrace();
+    }
+
+    this.onInitialFailed = onInitialFailed;
+    notifier = new Notifier(this::pingIPAddress);
+  }
+
+  public RadioPing() {
+    this((value) -> {});
+  }
+
+  private void pingIPAddress() {
+    try {
+      if (ipAddress.isReachable(250)) {
+        synchronized (this) {
+          receivedConnection = true;
+          responseCount++;
+        }
+      } else {
+        synchronized (this) {
+          initialFailed = true;
+        }
+      }
+    } catch (Exception e) {
+      e.printStackTrace();
+    }
+  }
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    notifier.startPeriodic(0.500);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    notifier.stop();
+    onInitialFailed.accept(initialFailed);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (!initialFailed) {
+      return receivedConnection;
+    } else {
+      return receivedConnection && responseCount >= 1;
+    }
+  }
+}

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

@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
+public class StartupConnectionCheck extends SequentialCommandGroup {
+  private boolean connectionFailed = false;
+
+  @Override
+  public boolean runsWhenDisabled() {
+    return true;
+  }
+
+  /** Creates a new StartupConnectionCheck. */
+  public StartupConnectionCheck(Command whilePinging, Command onSuccess, Command onFailed) {
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands(
+        new RadioPing()
+            .withTimeout(60.0)
+            .deadlineWith(whilePinging)
+            .finallyDo((interrupted) -> connectionFailed = interrupted),
+        Commands.either(onSuccess, onFailed, () -> !connectionFailed));
+  }
+}

src/main/java/frc/robot/commands/leds/SolidColor.java

@@ -13,6 +13,8 @@ public class SolidColor extends Command {
   private final Color color;
   private final LEDs leds;
 
+  private boolean ledsOn = false;
+
   /** Creates a new SolidColor. */
   public SolidColor(Color color, LEDs leds) {
     this.color = color;
@@ -29,17 +31,23 @@ public boolean runsWhenDisabled() {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    AddressableLEDBuffer buffer = leds.getBuffer();
-    for (int i = 0; i < buffer.getLength(); i++) {
-      buffer.setLED(i, color);
-    }
-
-    leds.updateBuffer();
+    ledsOn = false;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {}
+  public void execute() {
+    if (!ledsOn) {
+      AddressableLEDBuffer buffer = leds.getBuffer();
+      for (int i = 0; i < buffer.getLength(); i++) {
+        buffer.setLED(i, color);
+      }
+
+      leds.updateBuffer();
+    }
+
+    ledsOn = true;
+  }
 
   // Called once the command ends or is interrupted.
   @Override