7.6终终

src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java

@@ -1,11 +1,10 @@
 package frc.robot.subsystems;
 
 import com.ctre.phoenix6.controls.Follower;
-import com.ctre.phoenix6.controls.VoltageOut;
+import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.MotorAlignmentValue;
 
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -21,31 +20,35 @@ public class IntakeRollerSubsystem extends SubsystemBase {
     private final TalonFX leftIntakeRoller;
     private final TalonFX rightIntakeRoller;
 
-    private final VoltageOut voltageRequest = new VoltageOut(0);
+    private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
     private final Follower rightFollower;
     private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRoller");
+    private double targetSpeedRps = 0.0;
 
     public IntakeRollerSubsystem() {
         leftIntakeRoller = new TalonFX(Constants.Intaker.leftIntakeRollerID);
         rightIntakeRoller = new TalonFX(Constants.Intaker.rightIntakeRollerID);
+        leftIntakeRoller.getConfigurator().apply(Constants.Intaker.intakeRollerSlot0Configs);
+        rightIntakeRoller.getConfigurator().apply(Constants.Intaker.intakeRollerSlot0Configs);
         rightFollower = new Follower(leftIntakeRoller.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
         followLeft();
     }
 
     public void intake() {
-        setVoltage(Constants.Intaker.intakeRollerVoltage);
+        setSpeedByRPS(Constants.Intaker.intakeRollerIntakeSpeedRps);
     }
 
     public void outtake() {
-        setVoltage(Constants.Intaker.outtakeRollerVoltage);
+        setSpeedByRPS(Constants.Intaker.intakeRollerOuttakeSpeedRps);
     }
 
     public void stop() {
-        setVoltage(0);
+        setSpeedByRPS(0.0);
     }
 
-    public void setVoltage(double left) {
-        leftIntakeRoller.setControl(voltageRequest.withOutput(left));
+    public void setSpeedByRPS(double targetSpeedRps) {
+        this.targetSpeedRps = targetSpeedRps;
+        leftIntakeRoller.setControl(velocityRequest.withVelocity(targetSpeedRps));
         followLeft();
     }
 
@@ -64,5 +67,6 @@ public class IntakeRollerSubsystem extends SubsystemBase {
         table.getEntry("leftVelocity").setDouble(leftIntakeRoller.getVelocity().getValueAsDouble());
         table.getEntry("rightVelocity").setDouble(rightIntakeRoller.getVelocity().getValueAsDouble());
         table.getEntry("velocityDifference").setDouble(getVelocityDifference());
+        table.getEntry("targetSpeedRps").setDouble(targetSpeedRps);
     }
 }