New state machine PR

notes/superstructure.md

@@ -10,9 +10,13 @@ IDLE <--> |intakeReq + intakeEmpty| INTAKE
 
 READY <--> |intakeReq + empty| INTAKE
 
-READY --> |feedReq| FEED
+READY --> |feedReq| SPIN_UP_FEED
 
-READY --> |scoreReq| SCORE
+READY --> |scoreReq| SPIN_UP_SCORE
+
+SPIN_UP_FEED --> |atVelocity| FEED
+
+SPIN_UP_SCORE --> |atVelocity| SCORE
 
 
 FEED --> |flowReq| FEED_FLOW

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

@@ -158,7 +158,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, indexer, intake, shooter, driver, operator);
 
   private final Autos autos;
   private Optional<Alliance> lastAlliance = Optional.empty();

src/main/java/frc/robot/Superstructure.java

@@ -9,19 +9,29 @@
 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.indexer.IndexerSubsystem;
+import frc.robot.subsystems.intake.IntakeSubsystem;
+import frc.robot.subsystems.shooter.ShooterSubsystem;
 import frc.robot.subsystems.swerve.SwerveSubsystem;
 import frc.robot.utils.CommandXboxControllerSubsystem;
+import frc.robot.utils.FieldUtils.FeedTargets;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 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,
+    SPIN_UP_FEED,
+    FEED,
+    FEED_FLOW,
+    SPIN_UP_SCORE,
+    SCORE,
+    SCORE_FLOW,
+    SPIT;
     public final Trigger trigger;
 
     private SuperState() {
@@ -41,38 +51,195 @@ public Trigger getTrigger() {
   private Timer stateTimer = new Timer();
 
   private final SwerveSubsystem swerve;
+  private final IndexerSubsystem indexer;
+  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 = "Superstructure/Flowstate Request")
+  private Trigger flowReq;
+
+  @AutoLogOutput(key = "Superstructure/Anti Jam Req")
+  private Trigger antiJamReq;
+
+  @AutoLogOutput(key = "Superstructure/Is Full")
+  private Trigger isFull;
+
+  @AutoLogOutput(key = "Superstructure/Is Empty")
+  private Trigger isEmpty;
+
+  private boolean shouldFeed = false;
 
   // @AutoLogOutput(key = "Superstructure/At Extension?")
   // public Trigger atExtensionTrigger = new Trigger(this::atExtension).or(Robot::isSimulation);
 
   /** Creates a new Superstructure. */
   public Superstructure(
       SwerveSubsystem swerve,
+      IndexerSubsystem indexer,
+      IntakeSubsystem intake,
+      ShooterSubsystem shooter,
       CommandXboxControllerSubsystem driver,
       CommandXboxControllerSubsystem operator) {
     this.swerve = swerve;
+    this.indexer = indexer;
+    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);
+    // Toggles for feeding
+    operator.leftBumper().onTrue(Commands.runOnce(() -> shouldFeed = true));
+    operator.rightBumper().onTrue(Commands.runOnce(() -> shouldFeed = false));
+
+    scoreReq =
+        driver
+            .rightTrigger()
+            .and(DriverStation::isTeleop)
+            .and(() -> shouldFeed == false)
+            .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)
+            .and(() -> shouldFeed == true)
+            .or(Autos.autoFeedReq);
+
+    flowReq = driver.leftTrigger().and(driver.rightTrigger());
+
+    antiJamReq = driver.a().or(operator.a());
+
+    isFull = new Trigger(indexer::isFull);
+
+    isEmpty = new Trigger(indexer::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.SPIN_UP_FEED, feedReq);
+
+    bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq.and(isFull.negate()));
+
+    bindTransition(SuperState.READY, SuperState.SPIN_UP_SCORE, scoreReq);
+
+    bindTransition(
+        SuperState.SPIN_UP_SCORE,
+        SuperState.SCORE,
+        new Trigger(shooter::atFlywheelVelocitySetpoint));
+
+    bindTransition(
+        SuperState.SPIN_UP_FEED, SuperState.FEED, new Trigger(shooter::atFlywheelVelocitySetpoint));
+
+    bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty);
+
+    bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty);
 
-    scoreReq = driver.rightTrigger().negate().and(DriverStation::isTeleop).or(Autos.autoScoreReq);
+    // FEED_FLOW transitions
+    {
+      bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq);
+
+      bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate().and(feedReq));
+
+      bindTransition(
+          SuperState.FEED_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate()));
+
+      // No so sure about the end condition here.
+      bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty));
+    }
+
+    // SCORE_FLOW transitions
+    {
+      bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq);
+
+      bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate().and(scoreReq));
+
+      bindTransition(
+          SuperState.SCORE_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate()));
+
+      // No so sure about the end condition here.
+      bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty));
+    }
+
+    // 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(),
+        indexer.rest(),
+        shooter.rest()); // Maybe the indexer should be indexing?
+
+    bindCommands(SuperState.INTAKE, intake.intake(), indexer.index(), shooter.rest());
+
+    bindCommands(
+        SuperState.READY,
+        intake.rest(),
+        indexer.index(),
+        shooter.rest()); // Maybe index at slower speed?
+
+    bindCommands(
+        SuperState.SPIN_UP_SCORE, intake.rest(), indexer.rest(), shooter.shoot(swerve::getPose));
+
+    bindCommands(
+        SuperState.SPIN_UP_FEED,
+        intake.rest(),
+        indexer.rest(),
+        shooter.feed(
+            swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: SELECTION LOGIC
+
+    bindCommands(
+        SuperState.SCORE, intake.rest(), indexer.indexToShoot(), shooter.shoot(swerve::getPose));
+
+    bindCommands(
+        SuperState.SCORE_FLOW, intake.intake(), indexer.index(), shooter.shoot(swerve::getPose));
+
+    bindCommands(
+        SuperState.FEED,
+        intake.rest(),
+        indexer.index(),
+        shooter.feed(
+            swerve::getPose,
+            () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: ADD SOME SELECTION LOGIC
+
+    bindCommands(
+        SuperState.FEED_FLOW,
+        intake.intake(),
+        indexer.index(),
+        shooter.feed(swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose()));
+
+    bindCommands(SuperState.SPIT, intake.outake(), indexer.outtake(), shooter.spit());
   }
 
   public void periodic() {
@@ -105,6 +272,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 +307,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/indexer/IndexerSubsystem.java

@@ -65,15 +65,6 @@ public Command stopKicker() {
   public Command shoot() {
     return this.run(() -> kickerIO.setRollerVoltage(0));
   }
-  ;
-
-  public boolean isFull(boolean firstBeamBreak, boolean secondBeamBreak) {
-    if (firstBeamBreak && secondBeamBreak) {
-      return true;
-    } else {
-      return false;
-    }
-  }
 
   public boolean isEmpty() {
     return !firstCANRangeInputs.isDetected && !secondCANRangeInputs.isDetected;
@@ -87,20 +78,31 @@ public Command index() {
     return this.run(
         () -> {
           indexRollerIO.setRollerVoltage(5);
+          kickerIO.setRollerVoltage(-5);
         });
   }
 
-  public Command score() {
+  public Command indexToShoot() {
     return this.run(
         () -> {
           indexRollerIO.setRollerVoltage(10);
+          kickerIO.setRollerVoltage(5);
         });
   }
 
   public Command outtake() {
     return this.run(
         () -> {
           indexRollerIO.setRollerVoltage(-5);
+          kickerIO.setRollerVoltage(-5);
+        });
+  }
+
+  public Command rest() {
+    return this.run(
+        () -> {
+          indexRollerIO.setRollerVoltage(0.0);
+          kickerIO.setRollerVoltage(0.0);
         });
   }