fix(superstructure): Add some intermediate rules.

Author: haydenheroux

simgui-ds.json

@@ -101,8 +101,7 @@
       "useGamepad": true
     },
     {
-      "guid": "78696e70757401000000000000000000",
-      "useGamepad": true
+      "guid": "Keyboard0"
     }
   ]
 }

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

@@ -81,7 +81,9 @@ private void configureBindings() {
     operatorController.rightTrigger().onTrue(superstructure.shoot());
 
     operatorController.a().onTrue(superstructure.amp());
+    operatorController.b().onTrue(superstructure.intake());
     operatorController.x().onTrue(superstructure.stow());
+    operatorController.y().onTrue(superstructure.shoot());
   }
 
   /**

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

@@ -223,6 +223,10 @@ public Command to(SuperstructureState goal) {
     return new ToGoal(goal);
   }
 
+  public Command initial() {
+    return to(SuperstructureState.INITIAL);
+  }
+
   public Command stow() {
     return to(SuperstructureState.STOW);
   }

src/main/java/frc/robot/superstructure/SuperstructureGoals.java

@@ -1,5 +1,7 @@
 package frc.robot.superstructure;
 
+import frc.robot.superstructure.SuperstructureConstants.PivotAngleConstants;
+import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants;
 import frc.robot.superstructure.SuperstructureConstants.WristAngleConstants;
 import java.util.LinkedList;
 import java.util.Queue;
@@ -8,19 +10,63 @@ public class SuperstructureGoals {
 
   private final Queue<SuperstructureState> goals;
 
-  public SuperstructureGoals(SuperstructureState start, SuperstructureState end) {
-    goals = new LinkedList<SuperstructureState>();
+  public static Queue<SuperstructureState> merge(
+      Queue<SuperstructureState> first, Queue<SuperstructureState> second) {
+    Queue<SuperstructureState> mergedGoals = new LinkedList<SuperstructureState>();
+
+    while (!first.isEmpty()) {
+      mergedGoals.add(first.poll());
+    }
+
+    while (!second.isEmpty()) {
+      mergedGoals.add(second.poll());
+    }
+
+    return mergedGoals;
+  }
+
+  public static Queue<SuperstructureState> generate(
+      SuperstructureState start, SuperstructureState end) {
+    Queue<SuperstructureState> goals = new LinkedList<SuperstructureState>();
+
+    boolean shoulderMove = !start.atShoulderAngleGoal(end);
 
-    if (!start.atShoulderAngleGoal(end)) {
+    // Shoulder move: HOME -> STOW; ANY -> AMP
+    // Move wrist to STOW first, then move shoulder
+    if (shoulderMove) {
       goals.add(start.withWristAngle(WristAngleConstants.STOW));
-      goals.add(
-          start
-              .withShoulderAngleOf(end)
-              .withWristAngle(WristAngleConstants.STOW)
-              .withPivotAngleOf(end));
+      goals.add(end.withWristAngle(WristAngleConstants.STOW));
+      goals.add(end);
+      return goals;
+    }
+
+    boolean shoulderStowed = start.atShoulderAngle(ShoulderAngleConstants.STOW);
+    boolean wristMove = !start.atWristAngleGoal(end);
+    boolean pivotMove = !start.atPivotAngleGoal(end);
+    boolean pivotUp = start.pivotAngleRotations().position > PivotAngleConstants.DOWN.getRotations();
+
+    // Stow position movement & wrist-intake collision avoidance
+    if (shoulderStowed && wristMove && pivotMove) {
+      if (pivotUp) {
+        // Pivot is up and needs to go down; move it first
+        goals.add(start.withPivotAngleOf(end));
+      } else {
+        // Pivot is down and needs to go up; move wrist out of way first
+        goals.add(start.withWristAngleOf(end));
+      }
     }
 
     goals.add(end);
+
+    return goals;
+  }
+
+  public SuperstructureGoals(Queue<SuperstructureState> goals) {
+    this.goals = goals;
+  }
+
+  public SuperstructureGoals(SuperstructureState start, SuperstructureState end) {
+    this(generate(start, end));
   }
 
   public SuperstructureState get() {

src/main/java/frc/robot/superstructure/SuperstructureState.java

@@ -229,6 +229,19 @@ public boolean atShoulderAngleGoal(SuperstructureState goal) {
         SuperstructureConstants.ShoulderAngleConstants.TOLERANCE.getRotations());
   }
 
+  /**
+   * Returns true if at the shoulder angle.
+   *
+   * @param other
+   * @return true if at the shoulder angle.
+   */
+  public boolean atShoulderAngle(Rotation2d other) {
+    return MathUtil.isNear(
+        this.shoulderAngleRotations().position,
+        other.getRotations(),
+        SuperstructureConstants.ShoulderAngleConstants.TOLERANCE.getRotations());
+  }
+
   /**
    * Returns true if at the wrist angle goal.
    *