From 8edf1ab77ec006717f4bf363da9324f589cee9a1 Mon Sep 17 00:00:00 2001 From: POBots-353 Date: Wed, 24 Jan 2024 17:40:22 -0500 Subject: [PATCH] Added startup connection checking --- src/main/java/frc/robot/RobotContainer.java | 8 ++ .../java/frc/robot/commands/RadioPing.java | 87 +++++++++++++++++++ .../commands/StartupConnectionCheck.java | 33 +++++++ .../frc/robot/commands/leds/SolidColor.java | 22 +++-- 4 files changed, 143 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/commands/RadioPing.java create mode 100644 src/main/java/frc/robot/commands/StartupConnectionCheck.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 43bc1a4..90efa7f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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(); } /** diff --git a/src/main/java/frc/robot/commands/RadioPing.java b/src/main/java/frc/robot/commands/RadioPing.java new file mode 100644 index 0000000..e28f975 --- /dev/null +++ b/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 onInitialFailed; + + /** Creates a new RadioPing. */ + public RadioPing(Consumer 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; + } + } +} diff --git a/src/main/java/frc/robot/commands/StartupConnectionCheck.java b/src/main/java/frc/robot/commands/StartupConnectionCheck.java new file mode 100644 index 0000000..01cfd8e --- /dev/null +++ b/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)); + } +} diff --git a/src/main/java/frc/robot/commands/leds/SolidColor.java b/src/main/java/frc/robot/commands/leds/SolidColor.java index f1f3e04..5a774e3 100644 --- a/src/main/java/frc/robot/commands/leds/SolidColor.java +++ b/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