diff --git a/src/main/java/frc/robot/constants/CoralHolderConstants.java b/src/main/java/frc/robot/constants/CoralHolderConstants.java index 4ecd25e..dd5852a 100644 --- a/src/main/java/frc/robot/constants/CoralHolderConstants.java +++ b/src/main/java/frc/robot/constants/CoralHolderConstants.java @@ -8,6 +8,9 @@ public class CoralHolderConstants { public static final Time OUTTAKE_DURATION = Seconds.of(0.5); public static final Time INTAKE_DURATION = Seconds.of(1); - public static final double THROTTLE = 0.2; - public static final FalconMotorSupplier MOTOR_ID = new FalconMotorSupplier(5).withInvert(); + public static final double SPEED_RPS = 1; + public static final FalconMotorSupplier MOTOR_ID = new FalconMotorSupplier(5) + .withPID(1, 0, 0) + .withEncoder(9) + .withInvert(); } diff --git a/src/main/java/frc/robot/subsystems/CoralHolderSub.java b/src/main/java/frc/robot/subsystems/CoralHolderSub.java index ee90bd5..fdd9279 100644 --- a/src/main/java/frc/robot/subsystems/CoralHolderSub.java +++ b/src/main/java/frc/robot/subsystems/CoralHolderSub.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import com.ctre.phoenix6.controls.MotionMagicVelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.util.sendable.Sendable; @@ -11,21 +12,22 @@ public class CoralHolderSub extends SubsystemBase { private final TalonFX motor = CoralHolderConstants.MOTOR_ID.get(); + private final MotionMagicVelocityDutyCycle control = new MotionMagicVelocityDutyCycle(0); public CoralHolderSub() { setupSmartDash(); } public void forward() { - motor.set(CoralHolderConstants.THROTTLE); + motor.setControl(control.withVelocity(CoralHolderConstants.SPEED_RPS)); } public void reverse() { - motor.set(-CoralHolderConstants.THROTTLE); + motor.setControl(control.withVelocity(-CoralHolderConstants.SPEED_RPS)); } public void stop() { - motor.set(0); + motor.setControl(control.withVelocity(0)); } public Command manualOutCommand() { diff --git a/src/main/java/frc/robot/utils/TypeSupliers/motorsupplier/FalconMotorSupplier.java b/src/main/java/frc/robot/utils/TypeSupliers/motorsupplier/FalconMotorSupplier.java index ca0915c..b882f7a 100644 --- a/src/main/java/frc/robot/utils/TypeSupliers/motorsupplier/FalconMotorSupplier.java +++ b/src/main/java/frc/robot/utils/TypeSupliers/motorsupplier/FalconMotorSupplier.java @@ -28,6 +28,14 @@ public FalconMotorSupplier withInvert() { return this; } + public FalconMotorSupplier withFeedForward(double s, double v, double a) { + motorConfig.Slot0.kS = s; + motorConfig.Slot0.kV = v; + motorConfig.Slot0.kA = a; + + return this; + } + public FalconMotorSupplier withPID(double p, double i, double d) { motorConfig.Slot0.kP = p; motorConfig.Slot0.kI = i;