Pit and Match Dashboard Updates

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,29 @@ 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

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() {

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

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

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

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<Integer> 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<Integer> getSendableChooser() {
+    return sendableChooser;
+  }
+
+  public boolean isUseVirtualSwitch() {
+    return useVirtualSwitch;
+  }
+
+  @Override
+  public Set<Measure> getMeasures() {
+    return Set.of(new Measure("usingVirtualSwitch", () -> this.useVirtualSwitch ? 1.0 : 0.0));
+  }
+}

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
   }
 }