feat: Use intake goal.

simgui.json

@@ -19,6 +19,7 @@
       "/SmartDashboard/Arm Mechanism": "Mechanism2d",
       "/SmartDashboard/Mechanism": "Mechanism2d",
       "/SmartDashboard/SendableChooser[0]": "String Chooser",
+      "/SmartDashboard/Superstructure": "Mechanism2d",
       "/SmartDashboard/VisionSystemSim-visionSystem/Sim Field": "Field2d",
       "/SmartDashboard/arm mech": "Mechanism2d"
     },
@@ -33,7 +34,7 @@
           "visible": true
         }
       },
-      "/SmartDashboard/Mechanism": {
+      "/SmartDashboard/Superstructure": {
         "window": {
           "visible": true
         }

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

@@ -82,9 +82,7 @@ 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/arm/Arm.java

@@ -76,7 +76,7 @@ public void setShoulderPosition(double positionRotations) {
     shoulderMotor.setPosition(positionRotations);
   }
 
-  public void setShoulderSetpoint(double positionRotations, double velocityRotationsPerSecond) {
+  public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
     shoulderMotor.setSetpoint(positionRotations, velocityRotationsPerSecond);
   }
 }

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

@@ -2,11 +2,8 @@
 
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
-import frc.robot.intake.IntakeConstants.RollerMotorConstants;
 import frc.robot.intake.RollerMotorIO.RollerMotorIOValues;
 
 /** Subsystem class for the intake subsystem. */
@@ -60,25 +57,7 @@ public double getRollerVelocity() {
     return rollerMotorValues.velocityRotationsPerSecond;
   }
 
-  /**
-   * Returns a command that intakes using the rollers.
-   *
-   * @return a command that intakes using the rollers.
-   */
-  public Command intake() {
-    return Commands.runEnd(
-        () -> rollerMotor.setSetpoint(RollerMotorConstants.INTAKE_VELOCITY),
-        () -> rollerMotor.setSetpoint(0.0));
-  }
-
-  /**
-   * Returns a command that outtakes using the rollers.
-   *
-   * @return a command that outtakes using the rollers.
-   */
-  public Command outtake() {
-    return Commands.runEnd(
-        () -> rollerMotor.setSetpoint(RollerMotorConstants.OUTTAKE_VELOCITY),
-        () -> rollerMotor.setSetpoint(0.0));
+  public void setSetpoint(double rollerVelocityRotationsPerSecond) {
+    rollerMotor.setSetpoint(rollerVelocityRotationsPerSecond);
   }
 }

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

@@ -5,7 +5,7 @@
 /** Constants for the intake subsystem. */
 public class IntakeConstants {
   /** Constants for the roller motor. */
-  public static class RollerMotorConstants {
+  public static class RollerConstants {
     /** Velocity to apply when intaking in rotations per second. */
     public static final double INTAKE_VELOCITY = 1.0;
 

src/main/java/frc/robot/shooter/Shooter.java

@@ -2,14 +2,10 @@
 
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import frc.lib.Subsystem;
 import frc.lib.Telemetry;
 import frc.robot.shooter.FlywheelMotorIO.FlywheelMotorIOValues;
 import frc.robot.shooter.SerializerMotorIO.SerializerMotorIOValues;
-import frc.robot.shooter.ShooterConstants.FlywheelConstants;
-import frc.robot.shooter.ShooterConstants.SerializerConstants;
 
 /** Subsystem class for the shooter subsystem. */
 public class Shooter extends Subsystem {
@@ -81,36 +77,9 @@ public double getSerializerVelocity() {
     return serializerMotorValues.velocityRotationsPerSecond;
   }
 
-  /**
-   * Intakes a note.
-   *
-   * @return a command that intakes a note.
-   */
-  public Command intake() {
-    return Commands.runEnd(
-        () -> serializerMotor.setSetpoint(SerializerConstants.INTAKE_VELOCITY),
-        () -> serializerMotor.setSetpoint(0.0));
-  }
-
-  /**
-   * Serializes a note.
-   *
-   * @return a command that serializes a note.
-   */
-  public Command serialize() {
-    return Commands.runEnd(
-        () -> serializerMotor.setSetpoint(SerializerConstants.SERIALIZE_VELOCITY),
-        () -> serializerMotor.setSetpoint(0.0));
-  }
-
-  /**
-   * Spins the flywheel.
-   *
-   * @return a command that spins the flywheel.
-   */
-  public Command spin() {
-    return Commands.runEnd(
-        () -> flywheelMotor.setSetpoint(FlywheelConstants.SHOOT_VELOCITY),
-        () -> flywheelMotor.setSetpoint(0.0));
+  public void setSetpoint(
+      double flywheelVelocityRotationsPerSecond, double serializerVelocityRotationsPerSecond) {
+    flywheelMotor.setSetpoint(flywheelVelocityRotationsPerSecond);
+    serializerMotor.setSetpoint(serializerVelocityRotationsPerSecond);
   }
 }

src/main/java/frc/robot/shooter/ShooterConstants.java

@@ -8,19 +8,25 @@ public static class SerializerConstants {
     /** Velocity to apply while intaking in rotations per second. */
     public static final double INTAKE_VELOCITY = 4.75;
 
-    /** Velocity to apply while serializing in rotations per second. */
-    public static final double SERIALIZE_VELOCITY = 4.75;
+    /** Velocity to apply while serializing for a speaker shot in rotations per second. */
+    public static final double SPEAKER_SERIALIZER_VELOCITY = 4.75;
 
-    /** Maximum tengential speed in meters per second. */
-    public static final double MAXIMUM_TANGENTIAL_SPEED = 4.75;
+    /** Velocity to apply while serializing for an amp shot in rotations per second. */
+    public static final double AMP_SERIALIZE_VELOCITY = 2.5;
+
+    /** Maximum tengential speed in rotations per second. */
+    public static final double MAXIMUM_SPEED = 4.75;
   }
 
   /** Constants for the flywheel motor used in the shooter subsystem. */
   public static class FlywheelConstants {
-    /** Velocity to apply while shooting in rotations per second. */
-    public static final double SHOOT_VELOCITY = 5.65;
+    /** Velocity to apply while shooting into the speaker in rotations per second. */
+    public static final double SPEAKER_VELOCITY = 5.65;
+
+    /** Velocity to apply while shooting into the amp in rotations per second. */
+    public static final double AMP_VELOCITY = 2.5;
 
-    /** Maximum tangential speed in meters per second. */
-    public static final double MAXIMUM_TANGENTIAL_SPEED = 5.65;
+    /** Maximum tangential speed in rotations per second. */
+    public static final double MAXIMUM_SPEED = 5.65;
   }
 }

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

@@ -9,7 +9,10 @@
 import frc.lib.Telemetry;
 import frc.robot.arm.Arm;
 import frc.robot.intake.Intake;
+import frc.robot.intake.IntakeConstants.RollerConstants;
 import frc.robot.shooter.Shooter;
+import frc.robot.shooter.ShooterConstants.FlywheelConstants;
+import frc.robot.shooter.ShooterConstants.SerializerConstants;
 
 /** Subsystem class for the superstructure subsystem. */
 public class Superstructure extends Subsystem {
@@ -141,8 +144,14 @@ public SuperstructureState getState() {
   private void updateSetpoint() {
     setpoint = SuperstructureState.nextSetpoint(setpoint, goal);
 
-    arm.setShoulderSetpoint(
+    arm.setSetpoint(
         setpoint.shoulderAngleRotations().position, setpoint.shoulderAngleRotations().velocity);
+
+    shooter.setSetpoint(
+        setpoint.flywheelVelocityRotationsPerSecond(),
+        setpoint.serializerVelocityRotationsPerSecond());
+
+    intake.setSetpoint(setpoint.rollerVelocityRotationsPerSecond());
   }
 
   public void setPosition(SuperstructureState state) {
@@ -151,41 +160,46 @@ public void setPosition(SuperstructureState state) {
 
   public void setGoal(SuperstructureState goal) {
     this.goal = goal;
-
-    // resetMotionProfile();
   }
 
-  // private void resetMotionProfile() {
-  //   setpoint = measurement;
-  // }
-
   public boolean at(SuperstructureState goal) {
     updateMeasurement();
 
     return measurement.at(goal);
   }
 
-  public Command to(SuperstructureState goal) {
+  private Command to(SuperstructureState goal) {
     return new ToGoal(goal);
   }
 
-  public Command initial() {
-    return to(SuperstructureState.INITIAL);
-  }
-
   public Command stow() {
     return to(SuperstructureState.STOW);
   }
 
   public Command intake() {
-    return to(SuperstructureState.INTAKE);
+    return to(SuperstructureState.INTAKE)
+        .andThen(
+            to(
+                SuperstructureState.INTAKE
+                    .withSerializerVelocity(SerializerConstants.INTAKE_VELOCITY)
+                    .withRollerVelocity(RollerConstants.INTAKE_VELOCITY)));
   }
 
   public Command shoot() {
-    return to(SuperstructureState.SHOOT);
+    return to(SuperstructureState.SHOOT)
+        .andThen(
+            to(SuperstructureState.SHOOT.withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY))
+                .withTimeout(1.0))
+        .andThen(
+            to(
+                SuperstructureState.SHOOT
+                    .withFlywheelVelocity(FlywheelConstants.SPEAKER_VELOCITY)
+                    .withSerializerVelocity(SerializerConstants.SPEAKER_SERIALIZER_VELOCITY)));
   }
 
   public Command amp() {
-    return to(SuperstructureState.AMP);
+    return to(SuperstructureState.AMP)
+        .andThen(to(SuperstructureState.AMP_SHOOT).withTimeout(1.0))
+        .andThen(to(SuperstructureState.AMP_SERIALIZE));
   }
 }

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

@@ -11,6 +11,7 @@
 import frc.lib.InterpolatableColor;
 import frc.robot.RobotConstants;
 import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
+import frc.robot.intake.IntakeConstants.RollerConstants;
 import frc.robot.shooter.ShooterConstants.FlywheelConstants;
 import frc.robot.shooter.ShooterConstants.SerializerConstants;
 
@@ -21,7 +22,7 @@ public class SuperstructureMechanism {
 
   private final Mechanism2d mechanism;
 
-  private MechanismLigament2d shoulder, flywheel, serializer;
+  private MechanismLigament2d shoulder, flywheel, serializer, rollers;
 
   private final double WIDTH =
       2
@@ -46,22 +47,23 @@ public class SuperstructureMechanism {
 
   private final double SERIALIZER_LENGTH = Units.inchesToMeters(7.5);
 
+  private final Translation2d ORIGIN_TO_ROLLERS =
+      new Translation2d(Units.inchesToMeters(5.75), Units.inchesToMeters(2.5));
+
   private final Color8Bit DEFAULT_COLOR = new Color8Bit(Color.kLightGray);
 
-  private final InterpolatableColor flywheelColor =
+  private final InterpolatableColor FLYWHEEL_COLOR =
       new InterpolatableColor(Color.kLightGray, Color.kSalmon);
-  private final InterpolatableColor serializerColor =
+
+  private final InterpolatableColor SERIALIZER_COLOR =
       new InterpolatableColor(Color.kLightGray, Color.kCornflowerBlue);
 
+  private final InterpolatableColor ROLLERS_COLOR =
+      new InterpolatableColor(Color.kLightGray, Color.kSpringGreen);
+
   private SuperstructureMechanism() {
     mechanism = new Mechanism2d(WIDTH, HEIGHT);
 
-    initializeArmMechanism();
-
-    initializeFramePerimeterMechanisms();
-  }
-
-  private void initializeArmMechanism() {
     Translation2d armRootTranslation = ORIGIN.plus(ORIGIN_TO_SHOULDER_BASE);
 
     double armThickness = Units.inchesToMeters(2) * 100;
@@ -91,9 +93,16 @@ private void initializeArmMechanism() {
         shoulder.append(
             new MechanismLigament2d(
                 "serializer", SERIALIZER_LENGTH, -35, armThickness, DEFAULT_COLOR));
-  }
 
-  private void initializeFramePerimeterMechanisms() {
+    Translation2d rollersRootTranslation = ORIGIN.plus(ORIGIN_TO_ROLLERS);
+
+    rollers =
+        mechanism
+            .getRoot("intake", rollersRootTranslation.getX(), rollersRootTranslation.getY())
+            .append(
+                new MechanismLigament2d(
+                    "rollers", Units.inchesToMeters(12), 0, armThickness, DEFAULT_COLOR));
+
     double framePerimeterThickness = Units.inchesToMeters(1) * 10;
 
     mechanism
@@ -136,16 +145,23 @@ public void updateSuperstructure(SuperstructureState state) {
 
     flywheel.setColor(
         new Color8Bit(
-            flywheelColor.sample(
+            FLYWHEEL_COLOR.sample(
                 Math.abs(state.flywheelVelocityRotationsPerSecond()),
                 0,
-                FlywheelConstants.MAXIMUM_TANGENTIAL_SPEED)));
+                FlywheelConstants.MAXIMUM_SPEED)));
 
     serializer.setColor(
         new Color8Bit(
-            serializerColor.sample(
+            SERIALIZER_COLOR.sample(
                 Math.abs(state.serializerVelocityRotationsPerSecond()),
                 0,
-                SerializerConstants.MAXIMUM_TANGENTIAL_SPEED)));
+                SerializerConstants.MAXIMUM_SPEED)));
+
+    rollers.setColor(
+        new Color8Bit(
+            ROLLERS_COLOR.sample(
+                Math.abs(state.rollerVelocityRotationsPerSecond()),
+                0,
+                RollerConstants.MAXIMUM_SPEED)));
   }
 }

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

@@ -4,6 +4,8 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import frc.robot.RobotConstants;
+import frc.robot.shooter.ShooterConstants.FlywheelConstants;
+import frc.robot.shooter.ShooterConstants.SerializerConstants;
 import frc.robot.superstructure.SuperstructureConstants.ShoulderAngleConstants;
 import java.util.Objects;
 
@@ -28,6 +30,12 @@ public record SuperstructureState(
 
   public static final SuperstructureState AMP = new SuperstructureState(ShoulderAngleConstants.AMP);
 
+  public static final SuperstructureState AMP_SHOOT =
+      AMP.withFlywheelVelocity(FlywheelConstants.AMP_VELOCITY);
+
+  public static final SuperstructureState AMP_SERIALIZE =
+      AMP_SHOOT.withSerializerVelocity(SerializerConstants.AMP_SERIALIZE_VELOCITY);
+
   /**
    * Creates a new superstructure state.
    *