From 0233010eceafabbd676004804cb7e65b5085f065 Mon Sep 17 00:00:00 2001 From: noahBana Date: Mon, 5 Feb 2024 19:22:10 -0500 Subject: [PATCH 1/2] add cube mode and set alliance shuffleboard command --- src/main/java/frc/robot/Robot.java | 20 +++++++++++++ .../robotState/SetAllianceCommand.java | 29 +++++++++++++++++++ .../constants/SuperStructureConstants.java | 4 +++ .../superStructure/SuperStructure.java | 16 +++++++++- 4 files changed, 68 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/robotState/SetAllianceCommand.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ba1b0bb9..ff9cb0e0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -8,8 +8,10 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.commands.robotState.SetAllianceCommand; import frc.robot.constants.BuildConstants; import frc.robot.constants.RobotConstants; import org.littletonrobotics.junction.LogFileUtil; @@ -70,12 +72,30 @@ public void robotInit() { } Logger.start(); + logger.info( + "Event: {}, Match Type: {}, Match #: {}, Replay #: {}", + DriverStation.getEventName(), + DriverStation.getMatchType(), + DriverStation.getMatchNumber(), + DriverStation.getReplayNumber()); + + m_robotContainer = new RobotContainer(); m_robotContainer.setIsEvent(isEvent); if (!isEvent) { m_robotContainer.configureTelemetry(); m_robotContainer.configurePitDashboard(); } + + Shuffleboard.getTab("Match") + .add("SetAllianceRed", new SetAllianceCommand(Alliance.Red, m_robotContainer)) + .withPosition(2, 0) + .withSize(1, 1); + + Shuffleboard.getTab("Match") + .add("SetAllianceRed", new SetAllianceCommand(Alliance.Blue, m_robotContainer)) + .withSize(1, 1) + .withPosition(2, 1); } @Override diff --git a/src/main/java/frc/robot/commands/robotState/SetAllianceCommand.java b/src/main/java/frc/robot/commands/robotState/SetAllianceCommand.java new file mode 100644 index 00000000..dadb5b5b --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/SetAllianceCommand.java @@ -0,0 +1,29 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +public class SetAllianceCommand extends Command { + public Alliance alliance; + public RobotContainer robotContainer; + private static Logger logger = LoggerFactory.getLogger(SetAllianceCommand.class); + + public SetAllianceCommand(Alliance alliance, RobotContainer robotContainer) { + this.alliance = alliance; + this.robotContainer = robotContainer; + } + + @Override + public void initialize() { + robotContainer.setAllianceColor(alliance); + logger.info("Manual Alliance color change: {}", alliance); + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/frc/robot/constants/SuperStructureConstants.java b/src/main/java/frc/robot/constants/SuperStructureConstants.java index de0e251e..f375098d 100644 --- a/src/main/java/frc/robot/constants/SuperStructureConstants.java +++ b/src/main/java/frc/robot/constants/SuperStructureConstants.java @@ -58,4 +58,8 @@ public class SuperStructureConstants { public static final double kWristStowSetPoint = 0.0; public static final double kElbowStowSetPoint = 0.0; public static final double kShooterStowSetPoint = 0.0; + + // CUBE + public static final double kWristCubeSetPoint = 0.0; + public static final double kElbowCubeSetPoint = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/superStructure/SuperStructure.java b/src/main/java/frc/robot/subsystems/superStructure/SuperStructure.java index ec8b8bde..1b644887 100644 --- a/src/main/java/frc/robot/subsystems/superStructure/SuperStructure.java +++ b/src/main/java/frc/robot/subsystems/superStructure/SuperStructure.java @@ -239,6 +239,17 @@ public void podiumShoot() { nextState = SuperStructureStates.PODIUM; } + public void cube() { + elbowSetpoint = SuperStructureConstants.kElbowCubeSetPoint; + wristSetpoint = SuperStructureConstants.kWristCubeSetPoint; + + elbowSubsystem.setPosition(elbowSetpoint); + logger.info("{} -> TRANSFER(CUBE)"); + flipMagazineOut = true; + curState = SuperStructureStates.TRANSFER; + nextState = SuperStructureStates.CUBE; + } + // Periodic @Override public void periodic() { @@ -287,6 +298,8 @@ public void periodic() { break; case SUBWOOFER: break; + case CUBE: + break; } org.littletonrobotics.junction.Logger.recordOutput("SuperStructState", curState.ordinal()); } @@ -315,6 +328,7 @@ public enum SuperStructureStates { STOW, PODIUM, PREP_PODIUM, - SUBWOOFER + SUBWOOFER, + CUBE } } From 711d72d6dfd5f26b13a8fa1ccc1db08c35258e9b Mon Sep 17 00:00:00 2001 From: noahBana Date: Mon, 5 Feb 2024 20:04:28 -0500 Subject: [PATCH 2/2] add autoswitch and sendablechooser auton options for match dashboard --- src/main/java/frc/robot/Robot.java | 11 +++-- src/main/java/frc/robot/RobotContainer.java | 21 ++++++++- .../ToggleVirtualSwitchCommand.java | 32 +++++++++++++ .../frc/robot/subsystems/auto/AutoSwitch.java | 45 +++++++++++++++++++ 4 files changed, 102 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/commands/robotState/ToggleVirtualSwitchCommand.java create mode 100644 src/main/java/frc/robot/subsystems/auto/AutoSwitch.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ff9cb0e0..b4c33dce 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -73,12 +73,11 @@ public void robotInit() { Logger.start(); logger.info( - "Event: {}, Match Type: {}, Match #: {}, Replay #: {}", - DriverStation.getEventName(), - DriverStation.getMatchType(), - DriverStation.getMatchNumber(), - DriverStation.getReplayNumber()); - + "Event: {}, Match Type: {}, Match #: {}, Replay #: {}", + DriverStation.getEventName(), + DriverStation.getMatchType(), + DriverStation.getMatchNumber(), + DriverStation.getReplayNumber()); m_robotContainer = new RobotContainer(); m_robotContainer.setIsEvent(isEvent); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 539b446e..aa3a7343 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,9 +20,11 @@ import frc.robot.commands.robotState.IntakeCommand; import frc.robot.commands.robotState.ReleaseNoteCommand; import frc.robot.commands.robotState.StowCommand; +import frc.robot.commands.robotState.ToggleVirtualSwitchCommand; import frc.robot.commands.robotState.VisionShootCommand; import frc.robot.controllers.FlyskyJoystick; import frc.robot.controllers.FlyskyJoystick.Button; +import frc.robot.subsystems.auto.AutoSwitch; import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.drive.Swerve; @@ -55,6 +57,7 @@ public class RobotContainer { private final ElbowSubsystem elbowSubsystem; private final ShooterSubsystem shooterSubsystem; private final ClimbSubsystem climbSubsystem; + private final AutoSwitch autoSwitch; private final XboxController xboxController = new XboxController(1); private final Joystick driveJoystick = new Joystick(0); @@ -74,6 +77,7 @@ public RobotContainer() { climbSubsystem = new ClimbSubsystem(); intakeSubsystem = new IntakeSubsystem(new IntakeIOFX()); magazineSubsystem = new MagazineSubsystem(new MagazineIOFX()); + autoSwitch = new AutoSwitch(); superStructure = new SuperStructure(wristSubsystem, elbowSubsystem, shooterSubsystem, magazineSubsystem); @@ -110,11 +114,26 @@ private void configureMatchDashboard() { .withProperties(Map.of("colorWhenFalse", "blue")) .withSize(2, 2) .withPosition(0, 0); - Shuffleboard.getTab("Match") .addBoolean("Have Note", () -> robotStateSubsystem.hasNote()) .withSize(1, 1) .withPosition(2, 0); + Shuffleboard.getTab("Match") + .add("VirtualAutonSwitch", autoSwitch.getSendableChooser()) + .withSize(1, 1) + .withPosition(1, 2); + Shuffleboard.getTab("Match") + .add("ToggleVirtualSwitch", new ToggleVirtualSwitchCommand(autoSwitch)) + .withSize(1, 1) + .withPosition(2, 2); + Shuffleboard.getTab("Match") + .addBoolean("VirtualSwitchUsed?", () -> autoSwitch.isUseVirtualSwitch()) + .withSize(1, 1) + .withPosition(3, 2); + // Shuffleboard.getTab("Match") + // .addString("AutoSwitchPos", () -> autoSwitch.getSwitchPos()) // does not exist yet + // .withSize(1, 1) + // .withPosition(0, 2); } public void configureTelemetry() { diff --git a/src/main/java/frc/robot/commands/robotState/ToggleVirtualSwitchCommand.java b/src/main/java/frc/robot/commands/robotState/ToggleVirtualSwitchCommand.java new file mode 100644 index 00000000..61db1473 --- /dev/null +++ b/src/main/java/frc/robot/commands/robotState/ToggleVirtualSwitchCommand.java @@ -0,0 +1,32 @@ +package frc.robot.commands.robotState; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.auto.AutoSwitch; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +public class ToggleVirtualSwitchCommand extends Command { + private AutoSwitch autoswitch; + private static Logger logger; + + public ToggleVirtualSwitchCommand(AutoSwitch autoswitch) { + this.autoswitch = autoswitch; + logger = LoggerFactory.getLogger(ToggleVirtualSwitchCommand.class); + } + + @Override + public void initialize() { + autoswitch.toggleVirtualSwitch(); + logger.info("toggledSwitch:command"); + } + + @Override + public boolean isFinished() { + return true; + } + + @Override + public boolean runsWhenDisabled() { + return true; + } +} diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java new file mode 100644 index 00000000..659ab7c3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems.auto; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import java.util.Set; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; +import org.strykeforce.telemetry.measurable.MeasurableSubsystem; +import org.strykeforce.telemetry.measurable.Measure; + +public class AutoSwitch extends MeasurableSubsystem { + public boolean useVirtualSwitch; + private static SendableChooser sendableChooser = new SendableChooser<>(); + public Logger logger = LoggerFactory.getLogger(AutoSwitch.class); + + public AutoSwitch() { + sendableChooser.addOption("TEST", 0x00); + sendableChooser.setDefaultOption("TEST", 0x01); + sendableChooser.setDefaultOption("TEST", 0x03); + SmartDashboard.putData("Auto Mode", sendableChooser); + } + + public void toggleVirtualSwitch() { + logger.info("toggledSwitch:function"); + if (useVirtualSwitch) { + useVirtualSwitch = false; + } else { + useVirtualSwitch = true; + } + // useVirtualSwitch = useVirtualSwitch ? false : true; + } + + public SendableChooser getSendableChooser() { + return sendableChooser; + } + + public boolean isUseVirtualSwitch() { + return useVirtualSwitch; + } + + @Override + public Set getMeasures() { + return Set.of(new Measure("usingVirtualSwitch", () -> this.useVirtualSwitch ? 1.0 : 0.0)); + } +}