Alpha State Machine

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;

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<Alliance> lastAlliance = Optional.empty();

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;
@@ -21,7 +24,14 @@ public class Superstructure {
    * screenshots of the robot in each state. There are also named positions in cad for each state.
    */
   public enum SuperState {
-    IDLE();
+    IDLE,
+    INTAKE,
+    READY,
+    FEED,
+    FEED_FLOW,
+    SCORE,
+    SCORE_FLOW,
+    SPIT;
     public final Trigger trigger;
 
     private SuperState() {
@@ -41,38 +51,148 @@ 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);
 
   /** 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());
+
+    isFull = new Trigger(routing::isFull);
 
-    scoreReq = driver.rightTrigger().negate().and(DriverStation::isTeleop).or(Autos.autoScoreReq);
+    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.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 +225,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 +260,6 @@ private void setSubstates() {}
   // public Command transitionAfterZeroing() {
   //  }
 
-  private void addTransitions() {}
-
   /**
    * <b>Only for setting initial state at the beginning of auto</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();
+  }
+}

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(() -> {});
+  }
+}

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