feat(arm): Combine intake and shooter commands.

src/main/java/frc/robot/RobotConstants.java

@@ -43,5 +43,5 @@ public enum Subsystem {
     VISION
   }
 
-  public static final Set<Subsystem> REAL_SUBSYSTEMS = EnumSet.of(Subsystem.ARM);
+  public static final Set<Subsystem> REAL_SUBSYSTEMS = EnumSet.of(Subsystem.ARM, Subsystem.INTAKE);
 }

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

@@ -71,20 +71,24 @@ private void configureBindings() {
     driverController.y().onTrue(odometry.tare());
     driverController.x().whileTrue(swerve.cross());
 
-    // operatorController
-    //     .leftTrigger()
-    //     .whileTrue(
-    //         Commands.parallel(arm.to(ArmState.INTAKE), intake.out())
-    //             .andThen(Commands.parallel(intake.intake(), shooter.intake()))).onFalse(Commands.runOnce(() -> {
-    //               intake.setPivotGoal(PivotMotorConstants.MAXIMUM_ANGLE);
-    //               arm.setGoal(ArmState.STOW);
-    //             }));
-
-    // operatorController
-    //     .rightTrigger()
-    //     .whileTrue(arm.to(ArmState.SHOOT).andThen(shooter.shoot())).onFalse(Commands.runOnce(() -> arm.setGoal(ArmState.STOW)));
-
-    operatorController.a().whileTrue(arm.to(ArmState.INTAKE)).onFalse(Commands.runOnce(() -> arm.setGoal(ArmState.STOW)));
+    Command toIntake = Commands.parallel(
+        Commands.waitUntil(intake::isNotStowed).andThen(arm.to(ArmState.INTAKE)),
+        intake.out());
+
+    Command toStow = Commands.runOnce(() -> {
+      arm.setGoal(ArmState.STOW);
+      intake.setPivotGoal(PivotMotorConstants.MAXIMUM_ANGLE);
+    });
+
+    operatorController
+        .leftTrigger()
+        .whileTrue(
+            toIntake 
+                .andThen(Commands.parallel(intake.intake(), shooter.intake()))).onFalse(toStow);
+
+    operatorController
+        .rightTrigger()
+        .whileTrue(arm.to(ArmState.SHOOT).andThen(shooter.shoot())).onFalse(Commands.runOnce(() -> arm.setGoal(ArmState.STOW)));
   }
 
   /**

src/main/java/frc/robot/arm/ArmConstants.java

@@ -90,18 +90,18 @@ public static class WristMotorConstants {
     /** Proportional gain in volts per rotation. */
     public static final double KP = 48.0;
 
-    /** Maximum speed of the shoulder joint in rotations per second. */
-    public static final double MAXIMUM_SPEED = 0.5;
+    /** Maximum speed of the wrist joint in rotations per second. */
+    public static final double MAXIMUM_SPEED = 2;
 
-    /** Maximum acceleration of the shoulder joint in rotations per second per second. */
+    /** Maximum acceleration of the wrist joint in rotations per second per second. */
     public static final double MAXIMUM_ACCELERATION =
-        MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.5);
+        MotionProfileCalculator.calculateAcceleration(MAXIMUM_SPEED, 0.25);
 
-    /** Maximum speed and acceleration of the shoulder joint. */
+    /** Maximum speed and acceleration of the wrist joint. */
     public static final TrapezoidProfile.Constraints CONSTRAINTS =
         new TrapezoidProfile.Constraints(MAXIMUM_SPEED, MAXIMUM_ACCELERATION);
 
-    /** Motion profile of the shoulder joint using constraints. */
+    /** Motion profile of the wrist joint using constraints. */
     public static final TrapezoidProfile MOTION_PROFILE = new TrapezoidProfile(CONSTRAINTS);
   }
 }

src/main/java/frc/robot/intake/Intake.java

@@ -105,6 +105,7 @@ public void addToShuffleboard(ShuffleboardTab tab) {
         "Setpoint (deg)", () -> Units.rotationsToDegrees(pivotSetpoint.position));
     pivot.addDouble(
         "Goal (deg)", () -> Units.rotationsToDegrees(pivotGoal.position));
+    pivot.addBoolean("Is Not Stowed?", this::isNotStowed);
 
     ShuffleboardLayout roller = Telemetry.addColumn(tab, "Roller");
 
@@ -158,6 +159,10 @@ private Command pivotTo(Rotation2d angle) {
     return runOnce(() -> setPivotGoal(angle)).andThen(Commands.waitUntil(this::atPivotGoal));
   }
 
+  public boolean isNotStowed() {
+    return Rotation2d.fromRotations(pivotMotorValues.positionRotations).getDegrees() < 50;
+  }
+
   public Command intake() {
     return Commands.run(() -> rollerMotor.setVoltage(RollerMotorConstants.INTAKE_VOLTAGE))
         .finallyDo(rollerMotor::stop);

src/main/java/frc/robot/intake/IntakeFactory.java

@@ -25,8 +25,8 @@ public static PivotMotorIO createPivotMotor() {
    * @return a roller motor.
    */
   public static RollerMotorIO createRollerMotor() {
-    if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE))
-      return new RollerMotorIOSparkMax();
+    // if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE))
+    //   return new RollerMotorIOSparkMax();
 
     return new RollerMotorIOSim();
   }