Turned Intake Methods to Command Factories

Author: Orcasphynx

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

@@ -1,9 +1,25 @@
 package frc.robot.subsystems.intake;
 
+import com.ctre.phoenix6.configs.Slot0Configs;
+
 public class IntakeConstants {
 
+    private IntakeConstants(){}
+
     //TODO: Change to actual ports
-    public static final int kIntakeRollerMotor = 2; 
-    public static final int kIntakeExtensionMotor = 3;
+    public static final int ROLLER_MOTOR_ID = 2; 
+    public static final int EXTENSION_MOTOR_ID = 3;
+
+    //TODO: Tune
+    public static final double ALLOWABLE_EXTENSION_ERROR = 0.1;
+    public static final double EXTENSION_kP = 0;
+    public static final double EXTENSION_kD = 0;
+    public static Slot0Configs createExtensionMotorSlot0Configs(){
+        Slot0Configs slot = new Slot0Configs();
+        slot.kP = EXTENSION_kP;
+        slot.kD = EXTENSION_kD;
+        return slot;
+    }
+
 
 }

src/main/java/frc/robot/subsystems/intake/IntakePreferences.java

@@ -1,8 +1,14 @@
 package frc.robot.subsystems.intake;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
 import frc.robot.preferences.DoublePreference;
 
 public class IntakePreferences {
-    public static DoublePreference rollerIntakeSpeed = new DoublePreference("Intake/Roller Intake Speed", 0.5);
+
+    private IntakePreferences(){};
+
+    public static DoublePreference rollerIntakeSpeed = new DoublePreference("Intake/Roller Intake Speed", 0.1);
     public static DoublePreference intakeCollectPosition = new DoublePreference("Intake/Stowed Intake Position", 3);
 }

src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java

@@ -1,36 +1,53 @@
 package frc.robot.subsystems.intake;
 
+import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
+
+import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC;
 import com.ctre.phoenix6.hardware.TalonFX;
 
-public class IntakeSubsystem {
-    private final TalonFX m_intakeRollerMotor;
-    private final TalonFX m_intakeExtensionMotor;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
+
+public class IntakeSubsystem extends SubsystemBase{
+    private final TalonFX rollerMotor;
+    private final TalonFX extensionMotor;
+
+    private double currentPositionTarget;
 
     public IntakeSubsystem(){
-        m_intakeRollerMotor = new TalonFX(IntakeConstants.kIntakeRollerMotor);
-        m_intakeExtensionMotor = new TalonFX(IntakeConstants.kIntakeExtensionMotor);
+        rollerMotor = new TalonFX(IntakeConstants.ROLLER_MOTOR_ID);
+        extensionMotor = new TalonFX(IntakeConstants.EXTENSION_MOTOR_ID);
+        extensionMotor.getConfigurator().apply(IntakeConstants.createExtensionMotorSlot0Configs());
+        extensionMotor.setPosition(0);
+        currentPositionTarget = 0;
+    }
+
+    public Command spinRollerCommand(){
+        return runOnce(() -> rollerMotor.set(IntakePreferences.rollerIntakeSpeed.getValue())).withName("Spin Intake Roller");
     }
 
-    public void spinRoller(){
-        m_intakeRollerMotor.set(IntakePreferences.rollerIntakeSpeed.getValue());
+    public Command stopRollerCommand(){
+        return runOnce(() -> rollerMotor.stopMotor()).withName("Stop Intake Roller");
     }
 
-    public void stopRoller(){
-        m_intakeRollerMotor.stopMotor();
+    private boolean atExtensionSetpoint(){
+        return Math.abs(extensionMotor.getPosition().getValueAsDouble() - currentPositionTarget) < IntakeConstants.ALLOWABLE_EXTENSION_ERROR;
     }
 
-    public void setIntakePosition(double position){
-        m_intakeExtensionMotor.setPosition(position);
+    public Command setIntakePositionCommand(double position){
+        currentPositionTarget = position;
+        return runOnce(() -> extensionMotor.setControl(new PositionTorqueCurrentFOC(position))).andThen(Commands.waitUntil(() -> atExtensionSetpoint()));
     }
 
-    public void collect(){
-        spinRoller();
-        setIntakePosition(IntakePreferences.intakeCollectPosition.getValue());
+    public Command collectCommand(){
+        return setIntakePositionCommand(IntakePreferences.intakeCollectPosition.getValue()).andThen(spinRollerCommand()).withName("Activate Intake Collection");
     }
 
-    public void stow(){
-        stopRoller();
-        setIntakePosition(0);
+    public Command stowCommand(){
+        return stopRollerCommand().andThen(setIntakePositionCommand(0.0)).withName("Stow Intake");
     }