RM_Autochooser

.gitignore

@@ -171,6 +171,7 @@ out/
 # Simulation GUI and other tools window save file
 networktables.json
 simgui.json
+simgui-ds.json
 *-window.json
 
 # Simulation data log directory

src/main/java/frc/robot/Robot.java

@@ -4,6 +4,7 @@
 
 package frc.robot;
 
+import java.util.Optional;
 import java.util.concurrent.atomic.AtomicReference;
 
 import org.littletonrobotics.junction.LogFileUtil;
@@ -13,6 +14,9 @@
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.utils.logging.commands.CommandLogger;
@@ -28,6 +32,13 @@ public class Robot extends LoggedRobot {
   private final RobotContainer robotContainer;
   private static final AtomicReference<RobotMode> mode = new AtomicReference<>(RobotMode.DISABLED);
 
+  private static Character autonomousWinner;
+
+  private static boolean hubActive;
+  private static Timer timer = new Timer();
+  private static Alliance autoWinner;
+
+  private static Optional<DriverStation.Alliance> allianceColor = Optional.empty();
 
   /**
    * This function is run when the robot is first started up and should be used for any
@@ -97,6 +108,14 @@ public void robotPeriodic() {
         if (Constants.ENABLE_LOGGING) {
             CommandLogger.get().log();
         }
+
+    // Gets the alliance color.
+    if (DriverStation.isDSAttached() && allianceColor.isEmpty()) {
+      allianceColor = DriverStation.getAlliance();
+      if (allianceColor.isPresent()) {
+        robotContainer.getAutoChooser().getProvider().forceRefresh();
+      }
+    }
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
@@ -121,6 +140,9 @@ public void autonomousInit() {
     if (autonomousCommand != null) {
       autonomousCommand.schedule();
     }
+
+    // Hub is always active during autonomous.
+    hubActive = true;
   }
 
   /** This function is called periodically during autonomous. */
@@ -137,17 +159,45 @@ public void teleopInit() {
         if (autonomousCommand != null) {
             autonomousCommand.cancel();
         }
+
+    // Start the timer to keep track of shifts.
+    timer.start();
   }
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+    // Check who won autonomous.
+    if (autonomousWinner == null) {
+      autonomousWinner = DriverStation.getGameSpecificMessage().charAt(0);
+      if (autonomousWinner != null) {
+        autoWinner = switch (autonomousWinner) {
+          case 'R' -> Alliance.Red;
+          case 'B' -> Alliance.Blue;
+          default -> throw new AssertionError("Data is corrupt - winner must be red or blue.");
+        };
+      }
+    } else {
+      // Determine whether the hub is active.
+      if (timer.hasElapsed(110)) {
+        hubActive = true; // end game
+      } else if (timer.hasElapsed(85)) {
+        hubActive = (allianceColor.get() == autoWinner); // shift 4
+      } else if (timer.hasElapsed(60)) {
+        hubActive = (allianceColor.get() != autoWinner); // shift 3
+      } else if (timer.hasElapsed(35)) {
+        hubActive = (allianceColor.get() == autoWinner); // shift 2
+      } else if (timer.hasElapsed(10)) {
+        hubActive = (allianceColor.get() != autoWinner); // shift 1
+      } else hubActive = true; // transition
+    }
+  }
 
   @Override
   public void testInit() {
- mode.set(RobotMode.TEST);
-        // Cancels all running commands at the start of test mode.
-        CommandScheduler.getInstance().cancelAll();  }
+    mode.set(RobotMode.TEST);
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();  }
 
   /** This function is called periodically during test mode. */
   @Override
@@ -160,4 +210,10 @@ public void simulationInit() {}
   /** This function is called periodically whilst in simulation. */
   @Override
   public void simulationPeriodic() {}
+
+  // getters
+  /** @return whether the hub is active */
+  public static boolean hubActive() {return hubActive;}
+  /** @return the alliance color */
+  public static Optional<DriverStation.Alliance> getAllianceColor() {return allianceColor;}
 }

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

@@ -8,6 +8,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.autochooser.chooser.AutoChooser2026;
 import frc.robot.commands.roller.SpinRoller;
 import frc.robot.commands.tilt.TiltDown;
 import frc.robot.commands.tilt.TiltUp;
@@ -26,6 +27,7 @@ public class RobotContainer {
   private final RollerSubsystem rollerSubsystem;
   private final TiltSubsystem tiltSubsystem;
   private RobotVisualizer robotVisualizer = null;
+  private final AutoChooser2026 autoChooser;
   // Replace with CommandPS4Controller or CommandJoystick if needed
       //new CommandXboxController(OperatorConstants.kDriverControllerPort);
 
@@ -52,6 +54,7 @@ public RobotContainer() {
         }
     configureBindings();
     putShuffleboardCommands();
+    autoChooser = new AutoChooser2026(null);
   }
 
   /**
@@ -99,4 +102,8 @@ public Command getAutonomousCommand() {
   public RobotVisualizer getRobotVisualizer() {
     return robotVisualizer;
   }
+
+  public AutoChooser2026 getAutoChooser() {
+    return autoChooser;
+  }
 }

src/main/java/frc/robot/autochooser/AutoAction.java

@@ -0,0 +1,37 @@
+package frc.robot.autochooser;
+
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.function.Function;
+import java.util.stream.Collectors;
+
+public enum AutoAction {
+  DO_NOTHING("Do Nothing"),
+  TWO_PIECE_HIGH("2 Piece L4"),
+  TWO_PIECE_LOW("2 Piece L2"),
+  ONE_PIECE("1 Piece"),
+  CROSS_THE_LINE("Cross The Line"),
+  INVALID("INVALID");
+  private final String name;
+  private static final HashMap<String, AutoAction> nameMap =
+      new HashMap<>(
+          Arrays.stream(AutoAction.values())
+              .collect(Collectors.toMap(AutoAction::getName, Function.identity())));
+
+  AutoAction(String name) {
+    this.name = name;
+  }
+
+  public String getName() {
+    return name;
+  }
+
+  @Override
+  public String toString() {
+    return getName();
+  }
+
+  public static AutoAction fromName(String name) {
+    return nameMap.get(name);
+  }
+}

src/main/java/frc/robot/autochooser/FieldLocation.java

@@ -0,0 +1,59 @@
+package frc.robot.autochooser;
+
+import static edu.wpi.first.wpilibj.DriverStation.Alliance;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import frc.robot.Robot;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.function.Function;
+import java.util.stream.Collectors;
+
+public enum FieldLocation {
+  ZERO(0, 0, 0, "Zero"),
+  INVALID(-1, -1, -1, "INVALID"),
+  LEFT(7.150, 7.000, 180, "NON Processor Side"),
+  MIDDLE(7.150, 4.500, 180, "Middle"),
+  RIGHT(7.150, 2.000, 180, "Processor Side");
+
+  private static final double RED_X_POS = 2.3876; // meters
+  public static final double HEIGHT_OF_FIELD = 8.05;
+  public static final double LENGTH_OF_FIELD = 17.548225;
+  private final double yPose;
+  private final double xPose;
+  private final double angle;
+  private final String name;
+  private static final HashMap<String, FieldLocation> nameMap =
+      new HashMap<>(
+          Arrays.stream(FieldLocation.values())
+              .collect(Collectors.toMap(FieldLocation::getShuffleboardName, Function.identity())));
+
+  FieldLocation(double xPos, double yPose, double angle, String name) {
+    this.xPose = xPos;
+    this.yPose = yPose;
+    this.angle = angle;
+    this.name = name;
+  }
+
+  public static FieldLocation fromName(String string) {
+    return nameMap.get(string);
+  }
+
+  public Pose2d getLocation() {
+    Alliance alliance = Robot.getAllianceColor().orElse(null);
+    if (alliance == null) {
+      return new Pose2d(INVALID.xPose, INVALID.yPose, Rotation2d.fromDegrees(INVALID.angle));
+    }
+    double x = (alliance == Alliance.Red) ? xPose + RED_X_POS + Units.inchesToMeters(36) : xPose;
+    double y = (alliance == Alliance.Red) ? yPose - 2 * (yPose - (HEIGHT_OF_FIELD / 2)) : yPose;
+    double radian =
+        (alliance == Alliance.Red) ? Math.toRadians(180 - angle) : Math.toRadians(angle);
+    return new Pose2d(x, y, Rotation2d.fromRadians(radian));
+  }
+
+  public String getShuffleboardName() {
+    return name;
+  }
+}

src/main/java/frc/robot/autochooser/chooser/AutoChooser.java

@@ -0,0 +1,16 @@
+package frc.robot.autochooser.chooser;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.autochooser.event.AutoEvent;
+
+/** interface for taking in a {@link AutoEvent} and returning the corresponding {@link Command} */
+public interface AutoChooser {
+  /**
+   * @return Command that corresponds to the selected {@link AutoEvent} from the {@link
+   *     frc.robot.autochooser.event.AutoEventProvider}
+   */
+  Command getAutoCommand();
+
+  Pose2d getStartingPosition();
+}

src/main/java/frc/robot/autochooser/chooser/AutoChooser2026.java

@@ -0,0 +1,59 @@
+package frc.robot.autochooser.chooser;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.autochooser.AutoAction;
+import frc.robot.autochooser.FieldLocation;
+import frc.robot.autochooser.event.AutoEvent;
+import frc.robot.autochooser.event.AutoEventProvider;
+import frc.robot.autochooser.event.AutoEventProviderIO;
+import frc.robot.utils.logging.commands.DoNothingCommand;
+import java.util.Map;
+
+public class AutoChooser2026 extends SubsystemBase implements AutoChooser {
+  private final Map<AutoEvent, Command> commandMap;
+  private final AutoEventProvider provider;
+  // private final SwerveDrivetrain drivetrain;
+
+  public AutoChooser2026(
+      AutoEventProviderIO providerIO
+      // SwerveDrivetrain drivetrain,
+      ) {
+    provider = new AutoEventProvider(providerIO, this::isValid);
+    // this.drivetrain = drivetrain;
+    commandMap =
+        Map.ofEntries(
+            Map.entry(
+                new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.LEFT), new DoNothingCommand()),
+            Map.entry(
+                new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.MIDDLE), new DoNothingCommand()),
+            Map.entry(
+                new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.RIGHT), new DoNothingCommand())
+            );
+  }
+
+  @Override
+  public Command getAutoCommand() {
+    return commandMap.get(
+        new AutoEvent(provider.getSelectedAction(), provider.getSelectedLocation()));
+  }
+
+  @Override
+  public Pose2d getStartingPosition() {
+    return provider.getSelectedLocation().getLocation();
+  }
+
+  protected boolean isValid(AutoAction action, FieldLocation location) {
+    return commandMap.containsKey(new AutoEvent(action, location));
+  }
+
+  @Override
+  public void periodic() {
+    provider.updateInputs();
+  }
+
+  public AutoEventProvider getProvider() {
+    return provider;
+  }
+}

src/main/java/frc/robot/autochooser/event/AutoChooserInputs.java

@@ -0,0 +1,35 @@
+package frc.robot.autochooser.event;
+
+import frc.robot.autochooser.AutoAction;
+import frc.robot.autochooser.FieldLocation;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+public class AutoChooserInputs implements LoggableInputs {
+  AutoAction action = AutoAction.INVALID;
+  FieldLocation location = FieldLocation.INVALID;
+  AutoAction defaultAction = AutoAction.INVALID;
+  FieldLocation defaultLocation = FieldLocation.INVALID;
+  AutoAction feedbackAction = AutoAction.INVALID;
+  FieldLocation feedbackLocation = FieldLocation.INVALID;
+
+  @Override
+  public void toLog(LogTable table) {
+    table.put("action", action);
+    table.put("location", location);
+    table.put("defaultAction", defaultAction);
+    table.put("defaultLocation", defaultLocation);
+    table.put("feedbackAction", feedbackAction);
+    table.put("feedbackLocation", feedbackLocation);
+  }
+
+  @Override
+  public void fromLog(LogTable table) {
+    action = table.get("action", action);
+    location = table.get("location", location);
+    defaultAction = table.get("defaultAction", defaultAction);
+    defaultLocation = table.get("defaultLocation", defaultLocation);
+    feedbackAction = table.get("feedbackAction", feedbackAction);
+    feedbackLocation = table.get("feedbackLocation", feedbackLocation);
+  }
+}