refactor: Remove intake pivot.

src/main/java/frc/robot/RobotMechanisms.java

@@ -10,8 +10,6 @@
 import edu.wpi.first.wpilibj.util.Color8Bit;
 import frc.lib.InterpolatableColor;
 import frc.robot.arm.ArmConstants.ShoulderMotorConstants;
-import frc.robot.intake.IntakeConstants.PivotMotorConstants;
-import frc.robot.intake.IntakeConstants.RollerMotorConstants;
 import frc.robot.shooter.ShooterConstants.FlywheelConstants;
 import frc.robot.shooter.ShooterConstants.SerializerConstants;
 import frc.robot.superstructure.SuperstructureState;
@@ -23,7 +21,7 @@ public class RobotMechanisms {
 
   private final Mechanism2d mechanism;
 
-  private MechanismLigament2d shoulder, flywheel, serializer, pivot;
+  private MechanismLigament2d shoulder, flywheel, serializer;
 
   private final double WIDTH =
       2
@@ -43,14 +41,11 @@ public class RobotMechanisms {
       new InterpolatableColor(Color.kLightGray, Color.kSalmon);
   private final InterpolatableColor serializerColor =
       new InterpolatableColor(Color.kLightGray, Color.kCornflowerBlue);
-  private final InterpolatableColor intakeColor =
-      new InterpolatableColor(Color.kLightGray, Color.kPaleGreen);
 
   private RobotMechanisms() {
     mechanism = new Mechanism2d(WIDTH, HEIGHT);
 
     initializeArmMechanism();
-    initializeIntakeMechanism();
 
     initializeFramePerimeterMechanisms();
   }
@@ -104,20 +99,6 @@ private void initializeFramePerimeterMechanisms() {
                 "framePerimeterRight_", HEIGHT, 90, framePerimeterThickness, DEFAULT_COLOR));
   }
 
-  private void initializeIntakeMechanism() {
-    double intakeThickness = Units.inchesToMeters(3) * 100;
-
-    pivot =
-        mechanism
-            .getRoot(
-                "intake",
-                ORIGIN.getX() + Units.inchesToMeters(13.164),
-                ORIGIN.getY() + Units.inchesToMeters(6.283))
-            .append(
-                new MechanismLigament2d(
-                    "intake_", PivotMotorConstants.DISTANCE, 0.0, intakeThickness, DEFAULT_COLOR));
-  }
-
   public static RobotMechanisms getInstance() {
     if (instance == null) {
       instance = new RobotMechanisms();
@@ -142,14 +123,6 @@ public void updateSuperstructure(SuperstructureState state) {
     Rotation2d shooterRotation = offsetWristRotation;
     Rotation2d serializerRotation = offsetWristRotation.plus(Rotation2d.fromDegrees(180));
 
-    pivot.setAngle(Rotation2d.fromRotations(state.pivotAngleRotations().position));
-    pivot.setColor(
-        new Color8Bit(
-            intakeColor.sample(
-                Math.abs(state.rollerVelocityRotationsPerSecond()),
-                0,
-                RollerMotorConstants.MAXIMUM_SPEED)));
-
     flywheel.setAngle(shooterRotation);
     flywheel.setColor(
         new Color8Bit(

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

@@ -1,15 +1,12 @@
 package frc.robot.intake;
 
-import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
-import edu.wpi.first.math.util.Units;
 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.PivotMotorIO.PivotMotorIOValues;
 import frc.robot.intake.RollerMotorIO.RollerMotorIOValues;
 
 /** Subsystem class for the intake subsystem. */
@@ -18,12 +15,6 @@ public class Intake extends Subsystem {
   /** Instance variable for the intake subsystem singleton. */
   private static Intake instance = null;
 
-  /** Pivot motor. */
-  private final PivotMotorIO pivotMotor;
-
-  /** Pivot motor values. */
-  private final PivotMotorIOValues pivotMotorValues = new PivotMotorIOValues();
-
   /** Roller motor. */
   private final RollerMotorIO rollerMotor;
 
@@ -32,10 +23,8 @@ public class Intake extends Subsystem {
 
   /** Creates a new instance of the intake subsystem. */
   private Intake() {
-    pivotMotor = IntakeFactory.createPivotMotor();
     rollerMotor = IntakeFactory.createRollerMotor();
 
-    pivotMotor.configure();
     rollerMotor.configure();
   }
 
@@ -54,42 +43,17 @@ public static Intake getInstance() {
 
   @Override
   public void periodic() {
-    pivotMotor.update(pivotMotorValues);
     rollerMotor.update(rollerMotorValues);
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
-    ShuffleboardLayout pivot = Telemetry.addColumn(tab, "Pivot");
-
-    pivot.addDouble(
-        "Position (deg)", () -> Units.rotationsToDegrees(getMeasuredPivotState().position));
-
     ShuffleboardLayout roller = Telemetry.addColumn(tab, "Roller");
 
     roller.addDouble("Current (A)", () -> rollerMotorValues.currentAmps);
     roller.addDouble("Velocity (rps)", () -> rollerMotorValues.velocityRotationsPerSecond);
   }
 
-  /**
-   * Returns the intake pivot's measured state.
-   *
-   * @return the intake pivot's measured state.
-   */
-  public State getMeasuredPivotState() {
-    pivotMotor.update(pivotMotorValues);
-
-    return new State(
-        pivotMotorValues.positionRotations, pivotMotorValues.velocityRotationsPerSecond);
-  }
-
-  public void setPivotPosition(double positionRotations) {
-    pivotMotor.setPosition(positionRotations);
-  }
-
-  public void setPivotSetpoint(double positionRotations, double velocityRotationsPerSecond) {
-    pivotMotor.setSetpoint(positionRotations, velocityRotationsPerSecond);
-  }
 
   public double getRollerVelocity() {
     rollerMotor.update(rollerMotorValues);

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

@@ -5,16 +5,6 @@
 
 /** Constants for the intake subsystem. */
 public class IntakeConstants {
-
-  /** Constants for the pivot motor. */
-  public static class PivotMotorConstants {
-    /** Distance between the pivot and the far edge of the intake. */
-    public static final double DISTANCE = Units.inchesToMeters(10.275);
-
-    /** Angle between the measured intake position and the mass's position. */
-    public static final Rotation2d MASS_OFFSET = Rotation2d.fromDegrees(-34.34);
-  }
-
   /** Constants for the roller motor. */
   public static class RollerMotorConstants {
     /** Velocity to apply when intaking in rotations per second. */

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

@@ -7,18 +7,6 @@
 /** Helper class for creating hardware for the intake subsystem. */
 public class IntakeFactory {
 
-  /**
-   * Creates a pivot motor.
-   *
-   * @return a pivot motor.
-   */
-  public static PivotMotorIO createPivotMotor() {
-    if (Robot.isReal() && RobotConstants.REAL_SUBSYSTEMS.contains(Subsystem.INTAKE))
-      return new PivotMotorIOTalonSRX();
-
-    return new PivotMotorIOSim();
-  }
-
   /**
    * Creates a roller motor.
    *