diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1455868..ce87288 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,12 +14,15 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; + +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.autochooser.FieldLocation; import frc.robot.constants.Constants; import frc.robot.utils.logging.commands.CommandLogger; @@ -124,6 +127,13 @@ public void robotPeriodic() { SmartDashboard.putBoolean("Hub Active?", hubActive()); } + // Puts data on the elastic dashboard + SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); + SmartDashboard.putBoolean("Hub Active?", hubActive()); + SmartDashboard.putString("Selected Action", + robotContainer.getAutoChooser().getCommandDescription()); + SmartDashboard.putString("Starting Location", location().toString()); + // Gets the alliance color. if (DriverStation.isDSAttached() && allianceColor.isEmpty()) { allianceColor = DriverStation.getAlliance(); @@ -247,8 +257,11 @@ public static Diagnostics getDiagnostics() { return diagnostics; } + // Getters public boolean hubActive() {return hubActive;} public static Optional allianceColor() {return allianceColor;} public static String allianceColorString() {return String.valueOf(allianceColor.orElse(null));} + public FieldLocation location() {return robotContainer.getAutoChooser().getLocation();} + public Pose2d getStartingLocation() {return location().getLocation();} -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f0a3242..840aef0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -20,12 +20,13 @@ import frc.robot.commands.feeder.SpinFeeder; import frc.robot.commands.drive.FakeVision; import frc.robot.commands.intake.SpinIntake; +import frc.robot.autochooser.AutoChooser; import frc.robot.commands.angler.AimAngler; import frc.robot.commands.shooter.SetShootingState; import frc.robot.constants.Constants; import frc.robot.subsystems.AnglerSubsystem; -import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ApriltagSubsystem; +import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.FeederSubsystem; import frc.robot.constants.ShootingState; import frc.robot.constants.ShootingState.ShootState; @@ -55,8 +56,9 @@ * subsystems, commands, and trigger mappings) should be declared here. */ public class RobotContainer { + // Instantiate the autochooser. + private final AutoChooser autoChooser = new AutoChooser(); // The robot's subsystems and commands are defined here... - //private final RollerSubsystem rollerSubsystem; //private final TiltSubsystem tiltSubsystem; private final AnglerSubsystem anglerSubsystem; private final IntakeSubsystem intakeSubsystem; @@ -70,7 +72,7 @@ public class RobotContainer { private final CommandJoystick steerJoystick = new CommandJoystick(Constants.STEER_JOYSTICK_PORT); private ShootingState shootState = new ShootingState(ShootState.STOPPED); - // Replace with CommandPS4Controller or CommandJoystick if needed + // Replace with CommandPS4Controller or CommandJoystick if needed //new CommandXboxController(OperatorConstants.kDriverControllerPort);private final CommandXboxController controller = new CommandXboxController(Constants.XBOX_CONTROLLER_PORT); /** @@ -183,7 +185,7 @@ public void putShuffleboardCommands() { "Tilt Up", new TiltUp(tiltSubsystem)); - SmartDashboard.putData( + SmartDashboard.putData( "Tilt Down", new TiltDown(tiltSubsystem));*/ @@ -250,8 +252,9 @@ public void putShuffleboardCommands() { SmartDashboard.putData("AddApriltagReading", new AddApriltagReading(apriltagSubsystem,new ApriltagReading(0, 0, 0, 0, 0, 0, 0))); } + //basic drive command - if(!Constants.TESTBED){ + if(!Constants.TESTBED) { Command driveDirectionTime = new DriveDirectionTime(drivebase, 0.1,0.1, true, 1); SmartDashboard.putData("Drive Command", driveDirectionTime); SmartDashboard.putData("Fake vision", new FakeVision(drivebase)); @@ -265,12 +268,15 @@ public void putShuffleboardCommands() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - // An example command will be run in autonomous - return null; + return autoChooser.getCommand(); } public RobotVisualizer getRobotVisualizer() { - return robotVisualizer; + return robotVisualizer; + } + + public AutoChooser getAutoChooser() { + return autoChooser; } public IntakeSubsystem getIntakeSubsystem() { diff --git a/src/main/java/frc/robot/autochooser/AutoAction.java b/src/main/java/frc/robot/autochooser/AutoAction.java new file mode 100644 index 0000000..cbb8da9 --- /dev/null +++ b/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 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); + } +} diff --git a/src/main/java/frc/robot/autochooser/AutoChooser.java b/src/main/java/frc/robot/autochooser/AutoChooser.java new file mode 100644 index 0000000..877b9c6 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/AutoChooser.java @@ -0,0 +1,85 @@ +package frc.robot.autochooser; + +import java.util.HashMap; +import java.util.Map; + +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; + +import frc.robot.utils.logging.commands.LoggableCommand; + +public class AutoChooser { + + /** Drop-down chooser for the location. */ + private LoggedDashboardChooser locationChooser; + /** Drop-down chooser for the action. */ + private LoggedDashboardChooser actionChooser; + /** Structure for mapping possible choices to commands. */ + private final Map commandMap = new HashMap<>(); + + private final AutoCommand DEFAULT_COMMAND = AutoCommand.Invalid; + + public AutoChooser() { + this.locationChooser = new LoggedDashboardChooser<>( + "Location Chooser" + ); + this.actionChooser = new LoggedDashboardChooser<>( + "Action Chooser" + ); + populateChoosers(); + populateMap(); + } + + /** Populates the drop-down choosers with enum constants. */ + private void populateChoosers() { + for (FieldLocation location : FieldLocation.values()) { + switch (location) { + case INVALID -> {} // Skip the invalid case. + case ZERO -> { // Default + locationChooser.addDefaultOption(location.toString(), location); + } + default -> {locationChooser.addOption(location.toString(), location);} + }; + } + for (AutoAction action : AutoAction.values()) { + switch (action) { + case INVALID -> {} // Skip the invalid case. + case DO_NOTHING -> { // Default + actionChooser.addDefaultOption(action.toString(), action); + } + default -> {actionChooser.addOption(action.toString(), action);} + }; + } + } + + /** Put mappings here. + * @see AutoCommand */ + private void populateMap() { + // Currently, we have some example mappings. + commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.LEFT), + AutoCommand.DoNothing); + commandMap.put(new AutoEvent(AutoAction.DO_NOTHING, FieldLocation.RIGHT), + AutoCommand.DoSomething); + } + + private AutoCommand get() { + AutoAction chosenAction = actionChooser.get(); + FieldLocation chosenLocation = locationChooser.get(); + AutoEvent event = new AutoEvent(chosenAction, chosenLocation); + + return commandMap.getOrDefault(event, DEFAULT_COMMAND); + } + + public LoggableCommand getCommand() { + return get().getCommand(); + } + + /** @return A human-readable description of the selected command. */ + public String getCommandDescription() { + return get().getDescription(); + } + + public FieldLocation getLocation() { + return locationChooser.get(); + } + +} diff --git a/src/main/java/frc/robot/autochooser/AutoCommand.java b/src/main/java/frc/robot/autochooser/AutoCommand.java new file mode 100644 index 0000000..6d31ab4 --- /dev/null +++ b/src/main/java/frc/robot/autochooser/AutoCommand.java @@ -0,0 +1,47 @@ +package frc.robot.autochooser; + +import frc.robot.utils.logging.commands.LoggableCommand; +import frc.robot.utils.logging.commands.DoNothingCommand; +import frc.robot.utils.logging.commands.DoSomethingCommand; + +/** An enum to associate commands with human-readable descriptions. */ +public enum AutoCommand { + + // Add commands here. Importantly, you should give each command + // a readable description so that the drive team can tell what + // the robot will actually do. This will be used to give the + // drive team visual feedback on the elastic dashboard when + // selecting an autonoumous command. + Invalid( + "The selection is invalid. (The robot won't do anything.)", + new DoNothingCommand() + ), + DoNothing("The robot won't do anything.", new DoNothingCommand()), + DoSomething( + "Something will be printed to the terminal.", + new DoSomethingCommand(""" + SUCCESSFULLY + + DID + + SOMETHING + """) + ); + + private String description; + private LoggableCommand command; + + AutoCommand(String description, LoggableCommand command) { + this.description = description; + this.command = command; + } + + public String getDescription() { + return description; + } + + public LoggableCommand getCommand() { + return command; + } + +} diff --git a/src/main/java/frc/robot/autochooser/AutoEvent.java b/src/main/java/frc/robot/autochooser/AutoEvent.java new file mode 100644 index 0000000..38e4fef --- /dev/null +++ b/src/main/java/frc/robot/autochooser/AutoEvent.java @@ -0,0 +1,51 @@ +package frc.robot.autochooser; + +/** + * Wrapper Class, that Contains a {@link frc.robot.autochooser.AutoAction} and a {@link + * frc.robot.autochooser.FieldLocation} + */ +public class AutoEvent { + private final AutoAction action; + private final FieldLocation location; + + public AutoEvent(AutoAction action, FieldLocation location) { + this.action = action; + this.location = location; + } + + public AutoAction getAction() { + return action; + } + + public FieldLocation getLocation() { + return location; + } + + @Override + public int hashCode() { + final int prime = 31; + int result = 1; + result = prime * result + ((action == null) ? 0 : action.hashCode()); + result = prime * result + ((location == null) ? 0 : location.hashCode()); + return result; + } + + @Override + public boolean equals(Object obj) { + if (this == obj) + return true; + if (obj == null) + return false; + if (getClass() != obj.getClass()) + return false; + AutoEvent other = (AutoEvent) obj; + if (action != other.action) + return false; + if (location != other.location) + return false; + return true; + } + + + +} diff --git a/src/main/java/frc/robot/autochooser/FieldLocation.java b/src/main/java/frc/robot/autochooser/FieldLocation.java new file mode 100644 index 0000000..43e21ef --- /dev/null +++ b/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 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.allianceColor().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; + } +} diff --git a/src/main/java/frc/robot/utils/logging/commands/DoSomethingCommand.java b/src/main/java/frc/robot/utils/logging/commands/DoSomethingCommand.java new file mode 100644 index 0000000..06c9e61 --- /dev/null +++ b/src/main/java/frc/robot/utils/logging/commands/DoSomethingCommand.java @@ -0,0 +1,16 @@ +package frc.robot.utils.logging.commands; + +public class DoSomethingCommand extends DoNothingCommand { + + private String message; + + public DoSomethingCommand(String message) { + this.message = message; + } + + @Override + public void execute() { + System.out.println(message); + } + +}