Merge pull request #14 from Gongoliers/intake_1_25

Intake 1/25

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

@@ -66,7 +66,10 @@ private void configureBindings() {
     driverController.x().whileTrue(swerve.cross());
 
     operatorController.leftBumper().whileTrue(intake.intake()).whileTrue(shooter.intake());
-    operatorController.leftTrigger().whileTrue(shooter.smartIntake());
+    operatorController
+        .leftTrigger()
+        .whileTrue(intake.smartIntake())
+        .whileTrue(shooter.smartIntake());
 
     operatorController.rightBumper().whileTrue(intake.outtake()).whileTrue(shooter.shoot());
     operatorController.rightTrigger().whileTrue(shooter.smartShoot());

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

@@ -1,27 +1,33 @@
 package frc.robot.intake;
 
+import edu.wpi.first.math.filter.MedianFilter;
+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.robot.intake.IntakeConstants.IntakeMotorConstants;
-import frc.robot.intake.IntakeMotorIO.IntakeMotorIOValues;
+import frc.lib.Telemetry;
+import frc.robot.intake.IntakeConstants.IntakeCommandConstants;
+import frc.robot.intake.IntakeConstants.RollerMotorConstants;
+import frc.robot.intake.RollerMotorIO.RollerMotorIOValues;
 
 /** Subsystem class for the intake subsystem. */
 public class Intake extends Subsystem {
 
   /** Instance variable for the intake subsystem singleton. */
   private static Intake instance = null;
 
-  /** Intake motor. */
-  private final IntakeMotorIO intakeMotor;
+  /** Roller motor. */
+  private final RollerMotorIO rollerMotor;
 
-  /** Intake motor values. */
-  private final IntakeMotorIOValues intakeMotorValues = new IntakeMotorIOValues();
+  /** Roller motor values. */
+  private final RollerMotorIOValues rollerMotorValues = new RollerMotorIOValues();
+
+  private final MedianFilter rollerMotorCurrentFilter = new MedianFilter(3);
 
   /** Creates a new instance of the intake subsystem. */
   private Intake() {
-    intakeMotor = IntakeFactory.createIntakeMotor();
+    rollerMotor = IntakeFactory.createRollerMotor();
   }
 
   /**
@@ -39,21 +45,54 @@ public static Intake getInstance() {
 
   @Override
   public void periodic() {
-    intakeMotor.update(intakeMotorValues);
+    rollerMotor.update(rollerMotorValues);
+
+    rollerMotorCurrentFilter.calculate(rollerMotorValues.currentAmps);
   }
 
   @Override
   public void addToShuffleboard(ShuffleboardTab tab) {
-    tab.addDouble("Current", () -> intakeMotorValues.currentAmps);
+    ShuffleboardLayout roller = Telemetry.addColumn(tab, "Roller");
+
+    roller.addDouble("Current (A)", () -> rollerMotorValues.currentAmps);
+    roller.addDouble("Roller Velocity (rps)", this::getRollerVelocity);
+    roller.addBoolean("Current Spike?", this::rollerCurrentSpike);
   }
 
   public Command intake() {
-    return Commands.run(() -> intakeMotor.setVoltage(IntakeMotorConstants.INTAKE_VOLTAGE))
-        .finallyDo(intakeMotor::stop);
+    return Commands.run(() -> rollerMotor.setVoltage(RollerMotorConstants.INTAKE_VOLTAGE))
+        .finallyDo(rollerMotor::stop);
+  }
+
+  /**
+   * Returns true if the roller motor has a current spike.
+   *
+   * @return true if the roller motor has a current spike.
+   */
+  private boolean rollerCurrentSpike() {
+    return rollerMotorCurrentFilter.calculate(rollerMotorValues.currentAmps)
+        > RollerMotorConstants.NOTE_CURRENT;
+  }
+
+  /**
+   * Gets the velocity of the rollers in rotations per second.
+   *
+   * @return the velocity of the rollers in rotations per second.
+   */
+  private double getRollerVelocity() {
+    return rollerMotorValues.angularVelocityRotationsPerSecond / RollerMotorConstants.GEARING;
+  }
+
+  public Command smartIntake() {
+    return run(() -> rollerMotor.setVoltage(RollerMotorConstants.INTAKE_VOLTAGE))
+        .raceWith(
+            Commands.waitSeconds(IntakeCommandConstants.NOTE_DETECTION_DELAY)
+                .andThen(Commands.waitUntil(this::rollerCurrentSpike)))
+        .finallyDo(rollerMotor::stop);
   }
 
   public Command outtake() {
-    return Commands.run(() -> intakeMotor.setVoltage(-IntakeMotorConstants.INTAKE_VOLTAGE))
-        .finallyDo(intakeMotor::stop);
+    return Commands.run(() -> rollerMotor.setVoltage(-RollerMotorConstants.INTAKE_VOLTAGE))
+        .finallyDo(rollerMotor::stop);
   }
 }

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

@@ -1,22 +1,38 @@
 package frc.robot.intake;
 
+import edu.wpi.first.math.util.Units;
 import frc.lib.CAN;
 import frc.lib.MotorCurrentLimits;
 
 /** Constants for the intake subsystem. */
 public class IntakeConstants {
 
-  /** Constants for the intake motor. */
-  public static class IntakeMotorConstants {
-    /** Intake motor's CAN identifier. */
+  public static final double INTAKE_ROLLER_RADIUS = 0.5 * Units.inchesToMeters(1.375);
+
+  /** Constants for the roller motor. */
+  public static class RollerMotorConstants {
+    /** Roller motor's CAN identifier. */
     public static final CAN ID = new CAN(62);
 
-    /** If true, invert the intake motor. */
+    /** If true, invert the roller motor. */
     public static final boolean IS_INVERTED = false;
 
+    /** Gearing between the roller motor and the rollers. */
+    public static final double GEARING = 4.5;
+
     /** Voltage to apply when intaking in volts. */
     public static final double INTAKE_VOLTAGE = -10;
 
+    /** Current limits for the roller motor. */
     public static final MotorCurrentLimits CURRENT_LIMITS = new MotorCurrentLimits(40);
+
+    /** Size of the current spike when intaking a note in amps. */
+    public static final double NOTE_CURRENT = 18.0; // TODO
+  }
+
+  /** Constants for intake commands. */
+  public static class IntakeCommandConstants {
+    /** Delay between starting intaking and detecting notes in seconds. */
+    public static final double NOTE_DETECTION_DELAY = 1.0;
   }
 }

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

@@ -6,13 +6,13 @@
 public class IntakeFactory {
 
   /**
-   * Creates an intake motor.
+   * Creates a roller motor.
    *
-   * @return an intake motor.
+   * @return a roller motor.
    */
-  public static IntakeMotorIO createIntakeMotor() {
-    if (Robot.isReal()) return new IntakeMotorIOSparkMax();
+  public static RollerMotorIO createRollerMotor() {
+    if (Robot.isReal()) return new RollerMotorIOSparkMax();
 
-    return new IntakeMotorIOSim();
+    return new RollerMotorIOSim();
   }
 }

src/main/java/frc/robot/intake/IntakeMotorIO.java → src/main/java/frc/robot/intake/RollerMotorIO.java

@@ -1,34 +1,34 @@
 package frc.robot.intake;
 
 /** Intake motor hardware interface. */
-public interface IntakeMotorIO {
+public interface RollerMotorIO {
 
-  /** Values for the intake motor hardware interface. */
-  public static class IntakeMotorIOValues {
+  /** Values for the roller motor hardware interface. */
+  public static class RollerMotorIOValues {
     /** Angular velocity of the flywheel in rotations per second. */
     public double angularVelocityRotationsPerSecond = 0.0;
 
     /** Current drawn by the flywheel motor in amps. */
     public double currentAmps = 0.0;
   }
 
-  /** Configures the intake motor. */
+  /** Configures the roller motor. */
   public void configure();
 
   /**
-   * Updates the intake motor's values/
+   * Updates the roller motor's values.
    *
    * @param values
    */
-  public void update(IntakeMotorIOValues values);
+  public void update(RollerMotorIOValues values);
 
   /**
-   * Run the intake motor with the specified voltage.
+   * Run the roller motor with the specified voltage.
    *
    * @param volts the voltage to apply to the voltage motor.
    */
   public void setVoltage(double volts);
 
-  /** Stops the intake motor. */
+  /** Stops the roller motor. */
   public void stop();
 }

src/main/java/frc/robot/intake/IntakeMotorIOSim.java → src/main/java/frc/robot/intake/RollerMotorIOSim.java

@@ -1,13 +1,13 @@
 package frc.robot.intake;
 
-/** Simulated intake motor. */
-public class IntakeMotorIOSim implements IntakeMotorIO {
+/** Simulated roller motor. */
+public class RollerMotorIOSim implements RollerMotorIO {
 
   @Override
   public void configure() {}
 
   @Override
-  public void update(IntakeMotorIOValues values) {}
+  public void update(RollerMotorIOValues values) {}
 
   @Override
   public void setVoltage(double volts) {}

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

@@ -3,31 +3,31 @@
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.revrobotics.CANSparkMax;
 import frc.lib.Configurator;
-import frc.robot.intake.IntakeConstants.IntakeMotorConstants;
+import frc.robot.intake.IntakeConstants.RollerMotorConstants;
 
-/** Intake motor using a Spark Max. */
-public class IntakeMotorIOSparkMax implements IntakeMotorIO {
+/** Roller motor using a Spark Max. */
+public class RollerMotorIOSparkMax implements RollerMotorIO {
 
   /** Hardware Spark Max. */
   private final CANSparkMax sparkMax;
 
-  public IntakeMotorIOSparkMax() {
-    sparkMax = new CANSparkMax(IntakeMotorConstants.ID.id(), MotorType.kBrushless);
+  public RollerMotorIOSparkMax() {
+    sparkMax = new CANSparkMax(RollerMotorConstants.ID.id(), MotorType.kBrushless);
   }
 
   @Override
   public void configure() {
     Configurator.configureREV(sparkMax::restoreFactoryDefaults);
 
-    sparkMax.setInverted(IntakeMotorConstants.IS_INVERTED);
+    sparkMax.setInverted(RollerMotorConstants.IS_INVERTED);
 
     Configurator.configureREV(
         () ->
-            sparkMax.setSmartCurrentLimit((int) IntakeMotorConstants.CURRENT_LIMITS.breakerAmps()));
+            sparkMax.setSmartCurrentLimit((int) RollerMotorConstants.CURRENT_LIMITS.breakerAmps()));
   }
 
   @Override
-  public void update(IntakeMotorIOValues values) {
+  public void update(RollerMotorIOValues values) {
     values.angularVelocityRotationsPerSecond = sparkMax.getEncoder().getVelocity() / 60.0;
     values.currentAmps = sparkMax.getOutputCurrent();
   }

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

@@ -27,7 +27,8 @@ public class Shooter extends Subsystem {
   /** Beam break sensor values. */
   private final BeamBreakSensorIOValues beamBreakSensorValues = new BeamBreakSensorIOValues();
 
-  private final Debouncer beamBreakDebouncer =
+  /** Beam break sensor debouncer. */
+  private final Debouncer beamBreakSensorDebouncer =
       new Debouncer(
           SensorConstants.BEAM_BREAK_DEBOUNCE_PERIOD, SensorConstants.BEAM_BREAK_DEBOUNCE_TYPE);
 
@@ -67,7 +68,7 @@ public static Shooter getInstance() {
   public void periodic() {
     beamBreakSensor.update(beamBreakSensorValues);
 
-    beamBreakDebouncer.calculate(beamBreakSensorValues.isBroken);
+    beamBreakSensorDebouncer.calculate(beamBreakSensorValues.isBroken);
 
     serializerMotor.update(serializerMotorValues);
     flywheelMotor.update(flywheelMotorValues);
@@ -97,7 +98,7 @@ public void addToShuffleboard(ShuffleboardTab tab) {
    * @return true if the shooter is holding a note.
    */
   private boolean holdingNote() {
-    return beamBreakDebouncer.calculate(beamBreakSensorValues.isBroken);
+    return beamBreakSensorDebouncer.calculate(beamBreakSensorValues.isBroken);
   }
 
   /**