make conveyor subsystem (#7)

Co-authored-by: Aidan <>
team581 · Feb 11, 2024 · 22b98b9 · 22b98b9
1 parent 64ae0f2
commit 22b98b9
Show file tree

Hide file tree

Showing 6 changed files with 132 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/config/CompConfig.java b/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.ElevatorConfig;
 import frc.robot.config.RobotConfig.IMUConfig;
 import frc.robot.config.RobotConfig.IntakeConfig;
@@ -170,12 +171,21 @@ class CompConfig {
                   .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 QueuerConfig(
               0,
               0,
               new TalonFXConfiguration()
                   .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1))
-                  .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20))),
+                  .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20))
+                  .withMotorOutput(
+                      new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive))),
           new SwerveConfig(
               new CurrentLimitsConfigs().withStatorCurrentLimit(60),
               new CurrentLimitsConfigs().withStatorCurrentLimit(60),

diff --git a/src/main/java/frc/robot/config/PracticeConfig.java b/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.ElevatorConfig;
 import frc.robot.config.RobotConfig.IMUConfig;
 import frc.robot.config.RobotConfig.IntakeConfig;
@@ -150,18 +151,27 @@ class PracticeConfig {
           new IntakeConfig(
               16,
               0,
-              // Top Motor
               new TalonFXConfiguration()
                   .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1))
                   .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 QueuerConfig(
               0,
               0,
               new TalonFXConfiguration()
                   .withFeedback(new FeedbackConfigs().withSensorToMechanismRatio(1))
-                  .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20))),
+                  .withCurrentLimits(new CurrentLimitsConfigs().withSupplyCurrentLimit(20))
+                  .withMotorOutput(
+                      new MotorOutputConfigs()
+                          .withInverted(InvertedValue.CounterClockwise_Positive))),
           new SwerveConfig(
               new CurrentLimitsConfigs().withStatorCurrentLimit(80),
               new CurrentLimitsConfigs().withStatorCurrentLimit(80),

diff --git a/src/main/java/frc/robot/config/RobotConfig.java b/src/main/java/frc/robot/config/RobotConfig.java
@@ -19,6 +19,7 @@ public record RobotConfig(
     WristConfig wrist,
     ElevatorConfig elevator,
     IntakeConfig intake,
+    ConveyorConfig conveyor,
     QueuerConfig queuer,
     SwerveConfig swerve,
     IMUConfig imu,
@@ -40,6 +41,8 @@ public record IntakeConfig(
       int sensorID,
       TalonFXConfiguration motorConfig) {}
 
+  public record ConveyorConfig(int motorID, int sensorID, TalonFXConfiguration motorConfig) {}
+
   public record QueuerConfig(int motorID, int sensorID, TalonFXConfiguration motorConfig) {}
 
   public record WristConfig(

diff --git a/src/main/java/frc/robot/conveyor/ConveyorMode.java b/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;
+}
diff --git a/src/main/java/frc/robot/conveyor/ConveyorSubsystem.java b/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();
+  }
+}
diff --git a/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java b/src/main/java/frc/robot/util/scheduling/SubsystemPriority.java
@@ -22,6 +22,8 @@ public enum SubsystemPriority {
   INTAKE(10),
   QUEUER(10),
 
+  CONVEYOR(10),
+
   FMS(0),
   RUMBLE_CONTROLLER(0);