make conveyor subsystem

src/main/java/frc/robot/config/CompConfig.java

@@ -18,6 +18,7 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.config.RobotConfig.ClimberConfig;
+import frc.robot.config.RobotConfig.ConveyorConfig;
 import frc.robot.config.RobotConfig.IMUConfig;
 import frc.robot.config.RobotConfig.IntakeConfig;
 import frc.robot.config.RobotConfig.LightsConfig;
@@ -143,6 +144,13 @@ class CompConfig {
                   .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20))
                   .withMotorOutput(
                       new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))),
+          new ConveyorConfig(
+              0,
+              0,
+              new TalonFXConfiguration()
+                  .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1))
+                  .withSlot0(new Slot0Configs().withKP(0).withKV(0))
+                  .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(30))),
           new SwerveConfig(
               new CurrentLimitsConfigs().withStatorCurrentLimit(60),
               new CurrentLimitsConfigs().withStatorCurrentLimit(60),

src/main/java/frc/robot/config/PracticeConfig.java

@@ -17,6 +17,7 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import frc.robot.config.RobotConfig.ClimberConfig;
+import frc.robot.config.RobotConfig.ConveyorConfig;
 import frc.robot.config.RobotConfig.IMUConfig;
 import frc.robot.config.RobotConfig.IntakeConfig;
 import frc.robot.config.RobotConfig.LightsConfig;
@@ -132,6 +133,13 @@ class PracticeConfig {
                   .withMotorOutput(
                       new MotorOutputConfigs()
                           .withInverted(InvertedValue.CounterClockwise_Positive))),
+          new ConveyorConfig(
+              0,
+              0,
+              new TalonFXConfiguration()
+                  .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1))
+                  .withSlot0(new Slot0Configs().withKP(0).withKV(0))
+                  .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(30))),
           new SwerveConfig(
               new CurrentLimitsConfigs().withStatorCurrentLimit(80),
               new CurrentLimitsConfigs().withStatorCurrentLimit(80),

src/main/java/frc/robot/config/RobotConfig.java

@@ -18,6 +18,7 @@ public record RobotConfig(
     ClimberConfig climber,
     ShoulderConfig shoulder,
     IntakeConfig intake,
+    ConveyorConfig conveyor,
     SwerveConfig swerve,
     IMUConfig imu,
     LightsConfig lights) {
@@ -39,6 +40,8 @@ public record IntakeConfig(
       TalonFXConfiguration topMotorConfig,
       TalonFXConfiguration bottomMotorConfig) {}
 
+  public record ConveyorConfig(int motorID, int sensorID, TalonFXConfiguration motorConfig) {}
+
   public record ShoulderConfig(
       int rightMotorID,
       int leftMotorID,
@@ -67,7 +70,7 @@ public record SwerveConfig(
 
   // TODO: Change this to false during events
   public static final boolean IS_DEVELOPMENT = true;
-  private static final String PRACTICE_BOT_SERIAL_NUMBER = "0322443D"; //TODO: get serial number
+  private static final String PRACTICE_BOT_SERIAL_NUMBER = "0322443D"; // TODO: get serial number
   public static final String SERIAL_NUMBER = System.getenv("serialnum");
   public static final boolean IS_PRACTICE_BOT =
       SERIAL_NUMBER != null && SERIAL_NUMBER.equals(PRACTICE_BOT_SERIAL_NUMBER);

src/main/java/frc/robot/conveyor/ConveyorMode.java

@@ -0,0 +1,13 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.conveyor;
+
+public enum ConveyorMode {
+  IDLE,
+  PASSING_NOTE,
+  CONVEYORING,
+  AMP_OUTTAKE,
+  PASS_TO_INTAKE;
+}

src/main/java/frc/robot/conveyor/ConveyorSubsystem.java

@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.conveyor;
+
+import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle;
+import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.wpilibj.DigitalInput;
+import frc.robot.util.scheduling.LifecycleSubsystem;
+import frc.robot.util.scheduling.SubsystemPriority;
+
+public class ConveyorSubsystem extends LifecycleSubsystem {
+  private final TalonFX motor;
+  private final DigitalInput sensor;
+  private MotionMagicVelocityDutyCycle velocityRequest =
+      new MotionMagicVelocityDutyCycle(0.00).withSlot(0);
+
+  private ConveyorMode goalMode = ConveyorMode.IDLE;
+  private boolean hasNote = false;
+  private double goalVelocity = 0.00;
+
+  public ConveyorSubsystem(TalonFX motor, DigitalInput sensor) {
+    super(SubsystemPriority.CONVEYOR);
+
+    this.motor = motor;
+    this.sensor = sensor;
+  }
+
+  @Override
+  public void enabledPeriodic() {
+    if (goalMode == ConveyorMode.IDLE) {
+      motor.disable();
+    } else {
+      velocityRequest.withVelocity(goalVelocity);
+    }
+
+    setHasNote(sensorHasNote());
+  }
+
+  @Override
+  public void robotPeriodic() {
+    if (goalMode == ConveyorMode.AMP_OUTTAKE) {
+      goalVelocity = 0.20;
+    } else if (goalMode == ConveyorMode.CONVEYORING) {
+      goalVelocity = 0.35;
+    } else if (goalMode == ConveyorMode.PASSING_NOTE) {
+      goalVelocity = 0.30;
+    } else if (goalMode == ConveyorMode.PASS_TO_INTAKE) {
+      goalVelocity = -0.30;
+    } else {
+      goalVelocity = 0.00;
+    }
+  }
+
+  public void setMode(ConveyorMode mode) {
+    goalMode = mode;
+  }
+
+  public ConveyorMode getMode() {
+    return goalMode;
+  }
+
+  public boolean atGoal(ConveyorMode mode) {
+    if (goalMode != mode) {
+      return false;
+    }
+    if (goalMode == ConveyorMode.IDLE) {
+      return true;
+    }
+    if (hasNote) {
+      return true;
+    }
+    if (!hasNote && Math.abs(goalVelocity) > 0.00) {
+      return true;
+    }
+    return false;
+  }
+
+  public void setHasNote(boolean bool) {
+    hasNote = bool;
+  }
+
+  public boolean gethasNote() {
+    return hasNote;
+  }
+
+  private boolean sensorHasNote() {
+    return sensor.get();
+  }
+}

src/main/java/frc/robot/swerve/CommandSwerveDrivetrain.java

@@ -13,7 +13,6 @@
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
-import frc.robot.config.RobotConfig;
 import frc.robot.generated.PracticeBotTunerConstants;
 import java.util.function.Supplier;
 
@@ -22,14 +21,10 @@
  * in command-based projects easily.
  */
 public class CommandSwerveDrivetrain extends SwerveDrivetrain implements Subsystem {
-  public static final SwerveModuleConstants BackRight =
-      PracticeBotTunerConstants.BackRight;
-  public static final SwerveModuleConstants BackLeft =
-      PracticeBotTunerConstants.BackLeft;
-  public static final SwerveModuleConstants FrontRight =
-      PracticeBotTunerConstants.FrontRight;
-  public static final SwerveModuleConstants FrontLeft =
-      PracticeBotTunerConstants.FrontLeft;
+  public static final SwerveModuleConstants BackRight = PracticeBotTunerConstants.BackRight;
+  public static final SwerveModuleConstants BackLeft = PracticeBotTunerConstants.BackLeft;
+  public static final SwerveModuleConstants FrontRight = PracticeBotTunerConstants.FrontRight;
+  public static final SwerveModuleConstants FrontLeft = PracticeBotTunerConstants.FrontLeft;
   public static final SwerveDrivetrainConstants DrivetrainConstants =
       PracticeBotTunerConstants.DrivetrainConstants;
   private static final double kSimLoopPeriod = 0.005; // 5 ms

src/main/java/frc/robot/util/scheduling/SubsystemPriority.java

@@ -20,6 +20,8 @@ public enum SubsystemPriority {
   LIGHTS(10),
   INTAKE(10),
 
+  CONVEYOR(10),
+
   FMS(0),
   RUMBLE_CONTROLLER(0);