refactor(intake): Refactor intake.

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

@@ -161,9 +161,9 @@ public void clearProfile() {
   }
 
   /**
-   * Gets the measured state of the arm.
+   * Returns the arm's measured state.
    *
-   * @return the measured state of the arm.
+   * @return the arm's measured state.
    */
   public ArmState getMeasuredState() {
     shoulderMotor.update(shoulderMotorValues);

src/main/java/frc/robot/auto/Auto.java

@@ -135,8 +135,7 @@ public Command readyIntake() {
     double seconds = 3.0;
 
     return Commands.parallel(
-            Commands.waitUntil(intake::isNotStowed).andThen(arm.wristTo(ArmState.INTAKE)),
-            intake.out())
+            Commands.waitUntil(intake::isOut).andThen(arm.wristTo(ArmState.INTAKE)), intake.out())
         .withTimeout(seconds);
   }
 
@@ -149,16 +148,15 @@ public Command stow() {
 
     return Commands.parallel(
             arm.stow(),
-            Commands.waitUntil(() -> arm.at(ArmState.STOW)).withTimeout(2.0).andThen(intake.in()))
+            Commands.waitUntil(() -> arm.at(ArmState.STOW)).withTimeout(2.0).andThen(intake.stow()))
         .withTimeout(seconds);
   }
 
   public Command readyShoot() {
     double seconds = 3.0;
 
     return Commands.parallel(
-            Commands.waitUntil(intake::isNotStowed).andThen(arm.wristTo(ArmState.SHOOT)),
-            intake.out())
+            Commands.waitUntil(intake::isOut).andThen(arm.wristTo(ArmState.SHOOT)), intake.out())
         .withTimeout(seconds);
   }
 

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

@@ -1,8 +1,6 @@
 package frc.robot.intake;
 
 import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.MedianFilter;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
@@ -41,6 +39,7 @@ public class Intake extends Subsystem {
   /** Pivot motor setpoint. */
   private TrapezoidProfile.State pivotSetpoint;
 
+  /** Telemetry for the pivot trapezoid profile. */
   private final TrapezoidProfileTelemetry pivotProfileTelemetry;
 
   /** Roller motor. */
@@ -49,13 +48,6 @@ public class Intake extends Subsystem {
   /** Roller motor values. */
   private final RollerMotorIOValues rollerMotorValues = new RollerMotorIOValues();
 
-  /** Roller motor current median filter. */
-  private final MedianFilter rollerMotorCurrentFilter = new MedianFilter(3);
-
-  /** Roller motor current spike debouncer. */
-  private final Debouncer rollerMotorCurrentSpikeDebouncer =
-      new Debouncer(RollerMotorConstants.STALL_DURATION);
-
   /** Creates a new instance of the intake subsystem. */
   private Intake() {
     mechanism = RobotMechanisms.getInstance();
@@ -94,68 +86,101 @@ public static Intake getInstance() {
   public void periodic() {
     pivotMotor.update(pivotMotorValues);
 
-    updatePivotSetpoint();
+    // TODO Move to command
+    applyPivotSetpoint();
 
     rollerMotor.update(rollerMotorValues);
 
-    rollerMotorCurrentFilter.calculate(rollerMotorValues.currentAmps);
-
     mechanism.updateIntake(
         Rotation2d.fromRotations(pivotMotorValues.positionRotations), getRollerVelocity());
 
-    pivotProfileTelemetry.update(getMeasuredState(), getSetpoint(), getGoal());
+    pivotProfileTelemetry.update(getPivotMeasuredState(), getPivotSetpoint(), getPivotGoal());
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
     ShuffleboardLayout pivot = Telemetry.addColumn(tab, "Pivot");
 
-    pivot.addDouble("Position (deg)", () -> Units.rotationsToDegrees(getMeasuredState().position));
-    pivot.addDouble("Setpoint (deg)", () -> Units.rotationsToDegrees(getSetpoint().position));
-    pivot.addDouble("Goal (deg)", () -> Units.rotationsToDegrees(getGoal().position));
-    pivot.addBoolean("Is Not Stowed?", this::isNotStowed);
+    pivot.addDouble(
+        "Position (deg)", () -> Units.rotationsToDegrees(getPivotMeasuredState().position));
+    pivot.addDouble("Setpoint (deg)", () -> Units.rotationsToDegrees(getPivotSetpoint().position));
+    pivot.addDouble("Goal (deg)", () -> Units.rotationsToDegrees(getPivotGoal().position));
+    pivot.addBoolean("Is Not Stowed?", this::isOut);
 
     ShuffleboardLayout roller = Telemetry.addColumn(tab, "Roller");
 
     roller.addDouble("Current (A)", () -> rollerMotorValues.currentAmps);
     roller.addDouble(
         "Motor Velocity (rps)", () -> rollerMotorValues.angularVelocityRotationsPerSecond);
     roller.addDouble("Roller Velocity (rps)", this::getRollerVelocity);
-    roller.addBoolean("Current Spike?", this::rollerCurrentSpike);
-    roller.addBoolean("Stalled?", this::rollerStalled);
   }
 
-  public State getMeasuredState() {
+  /**
+   * Returns the intake pivot's measured state.
+   *
+   * @return the intake pivot's measured state.
+   */
+  public State getPivotMeasuredState() {
     pivotMotor.update(pivotMotorValues);
 
-    // TODO Include velocity
-    return new State(pivotMotorValues.positionRotations, 0.0);
+    return new State(
+        pivotMotorValues.positionRotations, pivotMotorValues.velocityRotationsPerSecond);
   }
 
-  public State getSetpoint() {
-    return pivotSetpoint;
+  /**
+   * Sets the intake pivot's goal.
+   *
+   * @param goal the intake pivot's goal.
+   */
+  public void setPivotGoal(Rotation2d goal) {
+    pivotGoal = new TrapezoidProfile.State(goal.getRotations(), 0);
   }
 
-  public State getGoal() {
+  /**
+   * Returns the intake pivot's goal.
+   *
+   * @return the intake pivot's goal.
+   */
+  public State getPivotGoal() {
     return pivotGoal;
   }
 
-  public void setPivotGoal(Rotation2d goal) {
-    pivotGoal = new TrapezoidProfile.State(goal.getRotations(), 0);
+  /**
+   * Returns the intake pivot's setpoint.
+   *
+   * @return the intake pivot's setpoint.
+   */
+  public State getPivotSetpoint() {
+    return pivotSetpoint;
   }
 
-  private void updatePivotSetpoint() {
+  /**
+   * Applies a setpoint to the intake pivot's controllers.
+   *
+   * <p>Calling this method alters the intake pivot's motion.
+   */
+  private void applyPivotSetpoint() {
     pivotSetpoint =
         PivotMotorConstants.MOTION_PROFILE.calculate(
             RobotConstants.PERIODIC_DURATION, pivotSetpoint, pivotGoal);
 
     pivotMotor.setSetpoint(pivotSetpoint.position, pivotSetpoint.velocity);
   }
 
+  /**
+   * Returns true if the intake pivot is at its goal.
+   *
+   * @return true if the intake pivot is at its goal.
+   */
   private boolean atPivotGoal() {
     return atPivotSetpoint() && pivotGoal.equals(pivotSetpoint);
   }
 
+  /**
+   * Returns true if the intake pivot is at its setpoint.
+   *
+   * @return true if the intake pivot is at its setpoint.
+   */
   private boolean atPivotSetpoint() {
     pivotMotor.update(pivotMotorValues);
 
@@ -165,44 +190,51 @@ private boolean atPivotSetpoint() {
         PivotMotorConstants.TOLERANCE.getRotations());
   }
 
-  public Command out() {
-    return pivotTo(PivotMotorConstants.MINIMUM_ANGLE).withTimeout(3.0);
-  }
-
-  public Command in() {
-    return pivotTo(PivotMotorConstants.MAXIMUM_ANGLE).withTimeout(3.0);
-  }
-
+  /**
+   * Returns a command that pivots the intake to an angle.
+   *
+   * @param angle the angle to pivot the intake to.
+   * @return a commadn that pivots the intake to an angle.
+   */
   private Command pivotTo(Rotation2d angle) {
     return runOnce(() -> setPivotGoal(angle)).andThen(Commands.waitUntil(this::atPivotGoal));
   }
 
-  public boolean isNotStowed() {
-    return pivotMotorValues.positionRotations < PivotMotorConstants.OUT_ANGLE.getRotations();
+  /**
+   * Returns a command that pivots the intake to the "out" position.
+   *
+   * @return a command that pivots the intake to the "out" position.
+   */
+  public Command out() {
+    return pivotTo(PivotMotorConstants.MINIMUM_ANGLE);
   }
 
-  public Command intake() {
-    return Commands.run(() -> rollerMotor.setVoltage(RollerMotorConstants.INTAKE_VOLTAGE))
-        .finallyDo(rollerMotor::stop);
+  /**
+   * Returns a command that pivots the intake to the "in" position.
+   *
+   * @return a command that pivots the intake to the "in" position.
+   */
+  public Command stow() {
+    return pivotTo(PivotMotorConstants.MAXIMUM_ANGLE);
   }
 
   /**
-   * Returns true if the roller motor has a current spike.
+   * Returns true if the intake is out (not stowed).
    *
-   * @return true if the roller motor has a current spike.
+   * @return true if the intake is out (not stowed).
    */
-  private boolean rollerCurrentSpike() {
-    return rollerMotorCurrentFilter.calculate(rollerMotorValues.currentAmps)
-        > RollerMotorConstants.NOTE_CURRENT;
+  public boolean isOut() {
+    return pivotMotorValues.positionRotations < PivotMotorConstants.OUT_ANGLE.getRotations();
   }
 
   /**
-   * Returns true if the roller motor is stalled.
+   * Returns a command that intakes using the rollers.
    *
-   * @return true if the roller motor is stalled.
+   * @return a command that intakes using the rollers.
    */
-  private boolean rollerStalled() {
-    return rollerMotorCurrentSpikeDebouncer.calculate(rollerCurrentSpike());
+  public Command intake() {
+    return Commands.run(() -> rollerMotor.setVoltage(RollerMotorConstants.INTAKE_VOLTAGE))
+        .finallyDo(rollerMotor::stop);
   }
 
   /**
@@ -214,15 +246,11 @@ private double getRollerVelocity() {
     return rollerMotorValues.angularVelocityRotationsPerSecond / RollerMotorConstants.GEARING;
   }
 
-  public Command smartIntake() {
-    return Commands.repeatingSequence(
-            run(() -> rollerMotor.setVoltage(RollerMotorConstants.INTAKE_VOLTAGE))
-                .until(this::rollerStalled),
-            run(() -> rollerMotor.setVoltage(RollerMotorConstants.OUTTAKE_VOLTAGE))
-                .until(() -> !rollerStalled()))
-        .finallyDo(rollerMotor::stop);
-  }
-
+  /**
+   * Returns a command that outtakes using the rollers.
+   *
+   * @return a command that outtakes using the rollers.
+   */
   public Command outtake() {
     return Commands.run(() -> rollerMotor.setVoltage(RollerMotorConstants.OUTTAKE_VOLTAGE))
         .finallyDo(rollerMotor::stop);

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

@@ -4,28 +4,18 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import frc.lib.MotionProfileCalculator;
-import frc.lib.MotorCurrentLimits;
 
 /** Constants for the intake subsystem. */
 public class IntakeConstants {
 
   /** Constants for the pivot motor. */
   public static class PivotMotorConstants {
-    /** If true, invert the pivot motor. */
-    public static final boolean IS_MOTOR_INVERTED = false;
-
-    /** If true, invert the pivot motor sensor. */
-    public static final boolean IS_SENSOR_INVERTED = true;
-
     /** Gearing between the pivot sensor and the pivot. */
     public static final double SENSOR_GEARING = 2.3;
 
     /** Gearing between the motor and the pivot. */
     public static final double MOTOR_GEARING = 49 * SENSOR_GEARING;
 
-    /** Pivot motor's moment of interia in kilograms meters squared. */
-    public static final double MOI = 0.02; // TODO
-
     /** Distance between the pivot and the far edge of the intake. */
     public static final double DISTANCE = Units.inchesToMeters(10.275);
 
@@ -71,25 +61,10 @@ public static class RollerMotorConstants {
     /** Voltage to apply when outtaking in volts. */
     public static final double OUTTAKE_VOLTAGE = -8;
 
-    /** Current limits for the roller motor. */
-    public static final MotorCurrentLimits CURRENT_LIMITS = new MotorCurrentLimits(40);
-
     /** Radius of the roller in meters. */
     public static final double INTAKE_ROLLER_RADIUS = 0.5 * Units.inchesToMeters(1.375);
 
-    /** Size of the current spike when intaking a note in amps. */
-    public static final double NOTE_CURRENT = 18.0; // TODO
-
-    /** Duration of the current spike when intaking a note. */
-    public static final double STALL_DURATION = 0.35;
-
     /** Maximum speed of the roller in rotations per second. */
     public static final double MAXIMUM_SPEED = 0.31;
   }
-
-  /** Constants for intake commands. */
-  public static class IntakeCommandConstants {
-    /** Delay between starting intaking and detecting notes in seconds. */
-    public static final double NOTE_DETECTION_DELAY = 0.5;
-  }
 }

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

@@ -7,6 +7,9 @@ public interface PivotMotorIO {
   public static class PivotMotorIOValues {
     /** Position of the pivot in rotations. */
     public double positionRotations = 0.0;
+
+    /** Velocity of the pivot in rotations per second. */
+    public double velocityRotationsPerSecond = 0.0;
   }
 
   /** Configures the pivot motor. */

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

@@ -6,17 +6,16 @@
 public class PivotMotorIOSim implements PivotMotorIO {
 
   private double positionRotations;
-
-  // private double velocityRotationsPerSecond;
+  private double velocityRotationsPerSecond;
 
   @Override
   public void configure() {}
 
   @Override
   public void update(PivotMotorIOValues values) {
     values.positionRotations = this.positionRotations;
-    // TODO Add velocity values
-    // values.velocityRotationsPerSecond = this.velocityRotationsPerSecond
+    ;
+    values.velocityRotationsPerSecond = this.velocityRotationsPerSecond;
   }
 
   @Override
@@ -28,7 +27,7 @@ public void setPosition(double positionRotations) {
   public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
     if (DriverStation.isEnabled()) {
       this.positionRotations = positionRotations;
-      // this.velocityRotationsPerSecond = velocityRotationsPerSecond;
+      this.velocityRotationsPerSecond = velocityRotationsPerSecond;
     }
   }
 }

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

@@ -35,8 +35,7 @@ public PivotMotorIOTalonSRX() {
   public void configure() {
     Configurator.configurePhoenix5(talonSRX::configFactoryDefault);
 
-    talonSRX.setSensorPhase(PivotMotorConstants.IS_SENSOR_INVERTED);
-    talonSRX.setInverted(PivotMotorConstants.IS_MOTOR_INVERTED);
+    talonSRX.setSensorPhase(true);
 
     Configurator.configurePhoenix5(
         () -> talonSRX.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative));
@@ -45,6 +44,7 @@ public void configure() {
   @Override
   public void update(PivotMotorIOValues values) {
     values.positionRotations = getPivotPosition();
+    values.velocityRotationsPerSecond = getPivotVelocity();
   }
 
   /**
@@ -69,6 +69,12 @@ private void setPivotPosition(double positionRotations) {
     Configurator.configurePhoenix5(() -> talonSRX.setSelectedSensorPosition(units));
   }
 
+  private double getPivotVelocity() {
+    double rotationsPerSecond = talonSRX.getSelectedSensorVelocity() / 2048.0;
+
+    return rotationsPerSecond / PivotMotorConstants.SENSOR_GEARING;
+  }
+
   @Override
   public void setPosition(double positionRotations) {
     setPivotPosition(positionRotations);

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

@@ -3,7 +3,6 @@
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import frc.lib.Configurator;
-import frc.robot.intake.IntakeConstants.RollerMotorConstants;
 
 /** Roller motor using a Spark Max. */
 public class RollerMotorIOSparkMax implements RollerMotorIO {
@@ -21,9 +20,7 @@ public void configure() {
 
     sparkMax.setInverted(false);
 
-    Configurator.configureREV(
-        () ->
-            sparkMax.setSmartCurrentLimit((int) RollerMotorConstants.CURRENT_LIMITS.breakerAmps()));
+    Configurator.configureREV(() -> sparkMax.setSmartCurrentLimit(40));
 
     Configurator.configureStatusFrames(sparkMax);
   }