Intake periodic update

Author: Orcasphynx

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

@@ -1,14 +1,11 @@
 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 {
 
     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);
+    public static DoublePreference intakeCollectPosition = new DoublePreference("Intake/Stowed Intake Position", 3); //in rotations
 }

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

@@ -1,28 +1,33 @@
 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;
 
 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;
+    private PositionTorqueCurrentFOC positionControl;
 
     public IntakeSubsystem(){
         rollerMotor = new TalonFX(IntakeConstants.ROLLER_MOTOR_ID);
         extensionMotor = new TalonFX(IntakeConstants.EXTENSION_MOTOR_ID);
         extensionMotor.getConfigurator().apply(IntakeConstants.createExtensionMotorSlot0Configs());
         extensionMotor.setPosition(0);
         currentPositionTarget = 0;
+        positionControl = new PositionTorqueCurrentFOC(0);
+    }
+
+    @Override
+    public void periodic(){
+        extensionMotor.setControl(positionControl.withPosition(currentPositionTarget));
     }
 
     public Command spinRollerCommand(){
@@ -38,16 +43,15 @@ private boolean atExtensionSetpoint(){
     }
 
     public Command setIntakePositionCommand(double position){
-        currentPositionTarget = position;
-        return runOnce(() -> extensionMotor.setControl(new PositionTorqueCurrentFOC(position))).andThen(Commands.waitUntil(() -> atExtensionSetpoint()));
+        return runOnce(() -> currentPositionTarget = position).andThen(Commands.waitUntil(() -> atExtensionSetpoint()));
     }
 
     public Command collectCommand(){
         return setIntakePositionCommand(IntakePreferences.intakeCollectPosition.getValue()).andThen(spinRollerCommand()).withName("Activate Intake Collection");
     }
 
     public Command stowCommand(){
-        return stopRollerCommand().andThen(setIntakePositionCommand(0.0)).withName("Stow Intake");
+        return stopRollerCommand().andThen(setIntakePositionCommand(0)).withName("Stow Intake");
     }