diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index d7e1e8f..ebad639 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -25,7 +25,8 @@ public class Autos { // mehhhhhhh private static boolean autoPreScore; private static boolean autoScore; - private static boolean autoIntakeCoral; + private static boolean autoIntake; + private static boolean autoFeed; // private static boolean autoIntakeAlgae; @@ -37,9 +38,12 @@ public class Autos { public static Trigger autoScoreReq = new Trigger(() -> autoScore).and(DriverStation::isAutonomous); - @AutoLogOutput(key = "Superstructure/Auto Coral Intake Request") - public static Trigger autoIntakeCoralReq = - new Trigger(() -> autoIntakeCoral).and(DriverStation::isAutonomous); + @AutoLogOutput(key = "Superstructure/Auto Intake Request") + public static Trigger autoIntakeReq = + new Trigger(() -> autoIntake).and(DriverStation::isAutonomous); + + @AutoLogOutput(key = "Superstructure/Auto Feed Request") + public static Trigger autoFeedReq = new Trigger(() -> autoFeed).and(DriverStation::isAutonomous); public enum PathEndType { PLACEHOLDER; diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java deleted file mode 100644 index 56cbefd..0000000 --- a/src/main/java/frc/robot/BuildConstants.java +++ /dev/null @@ -1,17 +0,0 @@ -package frc.robot; - -/** Automatically generated file containing build version information. */ -public final class BuildConstants { - public static final String MAVEN_GROUP = ""; - public static final String MAVEN_NAME = "Code"; - public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 5; - public static final String GIT_SHA = "c1fc6bab72e96fdd448e7273995587d29fc17414"; - public static final String GIT_DATE = "2026-01-11 10:31:08 PST"; - public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2026-01-11 11:55:21 PST"; - public static final long BUILD_UNIX_TIME = 1768161321995L; - public static final int DIRTY = 0; - - private BuildConstants() {} -} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index c22819e..5358727 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -28,6 +28,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.RoutingSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.led.LEDIOReal; import frc.robot.subsystems.led.LEDSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; @@ -114,6 +117,10 @@ public enum RobotType { private final SwerveSubsystem swerve = new SwerveSubsystem(swerveSimulation, canivore); private final LEDSubsystem leds; + private final RoutingSubsystem routing = new RoutingSubsystem(); + private final IntakeSubsystem intake = new IntakeSubsystem(); + private final ShooterSubsystem shooter = new ShooterSubsystem(); + private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); @@ -126,7 +133,8 @@ public enum RobotType { @AutoLogOutput(key = "Robot/Zeroing Request") private Trigger zeroingReq = driver.b(); - private final Superstructure superstructure = new Superstructure(swerve, driver, operator); + private final Superstructure superstructure = + new Superstructure(swerve, routing, intake, shooter, driver, operator); private final Autos autos; private Optional lastAlliance = Optional.empty(); diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 91759fa..f5da6c0 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -9,6 +9,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.RoutingSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.CommandXboxControllerSubsystem; import org.littletonrobotics.junction.AutoLogOutput; @@ -16,12 +19,16 @@ public class Superstructure { - /** - * We should have a state for every single "pose" the robot will hit. See this document for - * screenshots of the robot in each state. There are also named positions in cad for each state. - */ + /** We should have a state for every single action the robot will perform. */ public enum SuperState { - IDLE(); + IDLE, + INTAKE, + READY, + FEED, + FEED_FLOW, + SCORE, + SCORE_FLOW, + SPIT; public final Trigger trigger; private SuperState() { @@ -41,15 +48,33 @@ public Trigger getTrigger() { private Timer stateTimer = new Timer(); private final SwerveSubsystem swerve; + private final RoutingSubsystem routing; + private final IntakeSubsystem intake; + private final ShooterSubsystem shooter; private final CommandXboxControllerSubsystem driver; private final CommandXboxControllerSubsystem operator; // Declare triggers - @AutoLogOutput(key = "Superstructure/Pre Score Request") - public Trigger preScoreReq; - @AutoLogOutput(key = "Superstructure/Score Request") - public Trigger scoreReq; + private Trigger scoreReq; + + @AutoLogOutput(key = "Superstructure/Intake Request") + private Trigger intakeReq; + + @AutoLogOutput(key = "Superstructure/Feed Request") + private Trigger feedReq; + + @AutoLogOutput(key = "Superstructre/Flowstate Request") + private Trigger flowReq; + + @AutoLogOutput(key = "Superstructre/Anti Jam Req") + private Trigger antiJamReq; + + @AutoLogOutput(key = "Superstructure/Is Full") + private Trigger isFull; + + @AutoLogOutput(key = "Superstructure/Is Empty") + private Trigger isEmpty; // @AutoLogOutput(key = "Superstructure/At Extension?") // public Trigger atExtensionTrigger = new Trigger(this::atExtension).or(Robot::isSimulation); @@ -57,22 +82,118 @@ public Trigger getTrigger() { /** Creates a new Superstructure. */ public Superstructure( SwerveSubsystem swerve, + RoutingSubsystem routing, + IntakeSubsystem intake, + ShooterSubsystem shooter, CommandXboxControllerSubsystem driver, CommandXboxControllerSubsystem operator) { this.swerve = swerve; + this.routing = routing; + this.intake = intake; + this.shooter = shooter; this.driver = driver; this.operator = operator; addTriggers(); addTransitions(); + addCommands(); stateTimer.start(); } private void addTriggers() { - preScoreReq = driver.rightTrigger().or(Autos.autoPreScoreReq); + // TODO: THESE BINDINGS WILL LIKELY CHANGE. SHOULD HAVE A FULL MEETING TO DISCUSS + scoreReq = + driver + .rightTrigger() + .and(DriverStation::isTeleop) + .or(Autos.autoScoreReq); // Maybe should include if its our turn? + + intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); + + feedReq = driver.rightBumper().and(DriverStation::isTeleop).or(Autos.autoFeedReq); + + flowReq = operator.rightTrigger(); + + antiJamReq = driver.a().or(operator.a()); - scoreReq = driver.rightTrigger().negate().and(DriverStation::isTeleop).or(Autos.autoScoreReq); + isFull = new Trigger(routing::isFull); + + isEmpty = new Trigger(routing::isEmpty); + } + + private void addTransitions() { + bindTransition(SuperState.IDLE, SuperState.INTAKE, intakeReq); + + bindTransition(SuperState.INTAKE, SuperState.IDLE, intakeReq.negate().and(isEmpty)); + + bindTransition( + SuperState.INTAKE, SuperState.READY, (intakeReq.negate().and(isEmpty.negate())).or(isFull)); + + bindTransition(SuperState.INTAKE, SuperState.FEED, feedReq); + + bindTransition(SuperState.INTAKE, SuperState.SCORE, scoreReq); + + bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq.and(isFull.negate())); + + bindTransition(SuperState.READY, SuperState.FEED, feedReq); + + bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); + + bindTransition(SuperState.READY, SuperState.SCORE, scoreReq); + + bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty); + + // FEED_FLOW transitions + { + bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq); + + // No so sure about the end condition here. + bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate()); + + // Maybe should be a transition from idle to flow as well? In case robot doesn't already have + // a fuel + } + + // SCORE_FLOW transitions + { + bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq); + + bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate()); + // Maybe should be a transition from idle to flow as well? In case robot doesn't already have + // a fuel + } + + // Transition from any state to SPIT for anti jamming + antiJamReq.onTrue(changeStateTo(SuperState.SPIT)); + + bindTransition(SuperState.SPIT, SuperState.IDLE, antiJamReq.negate()); + } + + private void addCommands() { + bindCommands( + SuperState.IDLE, + intake.rest(), + routing.rest(), + shooter.rest()); // Maybe the routing should be indexing? + + bindCommands(SuperState.INTAKE, intake.intake(), routing.index(), shooter.rest()); + + bindCommands( + SuperState.READY, + intake.rest(), + routing.index(), + shooter.rest()); // Maybe index at slower speed? + + bindCommands(SuperState.SCORE, intake.rest(), routing.index(), shooter.shoot()); + + bindCommands(SuperState.SCORE_FLOW, intake.intake(), routing.index(), shooter.shoot()); + + bindCommands(SuperState.FEED, intake.rest(), routing.index(), shooter.feed()); + + bindCommands(SuperState.FEED_FLOW, intake.intake(), routing.index(), shooter.feed()); + + bindCommands(SuperState.SPIT, intake.spit(), routing.reverseIndex(), shooter.shoot()); } public void periodic() { @@ -105,6 +226,16 @@ private void bindTransition(SuperState start, SuperState end, Trigger trigger, C trigger.and(start.getTrigger()).onTrue(Commands.parallel(changeStateTo(end), cmd)); } + /** + * Runs the passed in command(s) in parallel when the superstructure is in the passed in state + * + * @param state + * @param commands + */ + private void bindCommands(SuperState state, Command... commands) { + state.getTrigger().whileTrue(Commands.parallel(commands)); + } + // public boolean atExtension(SuperState state) { // } @@ -130,8 +261,6 @@ private void setSubstates() {} // public Command transitionAfterZeroing() { // } - private void addTransitions() {} - /** * Only for setting initial state at the beginning of auto * diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..80f7af6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +// TODO +public class IntakeSubsystem extends SubsystemBase { + public Command intake() { + // TODO + return idle(); + } + + // Can't call it idle bc idle is smthing else + public Command rest() { + // TODO + return idle(); + } + + public Command spit() { + // TODO + return idle(); + } +} diff --git a/src/main/java/frc/robot/subsystems/RoutingSubsystem.java b/src/main/java/frc/robot/subsystems/RoutingSubsystem.java new file mode 100644 index 0000000..8bd19e0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/RoutingSubsystem.java @@ -0,0 +1,36 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +// TODO!!! +public class RoutingSubsystem extends SubsystemBase { + + // TODO: BASE ON ACTUAL SENSOR READINGS + private boolean isFull; + private boolean isEmpty; + + public boolean isFull() { + return isFull; + } + + public boolean isEmpty() { + return isEmpty; + } + + // TODO!! + public Command index() { + return this.run(() -> {}); + } + + // Can't call it idle bc idle is smthing else + public Command rest() { + // TODO + return idle(); + } + + public Command reverseIndex() { + // TODO + return this.run(() -> {}); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..d3fad56 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterSubsystem extends SubsystemBase { + public Command shoot() { + return this.idle(); + } + + public Command feed() { + return this.idle(); + } + + public Command rest() { + return this.idle(); + } +}