diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index ed09a30..2b8b564 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -36,18 +36,18 @@ public class Intake extends SubsystemBase{ public void intakeDown(){ intakeRoller.set(1.0); - intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kDownAngle) + intakeFeedForward.calculate(IntakeConstants.kDownAngle, 0.0)); + intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kDownAngle) + intakeFeedForward.calculate(IntakeConstants.kDownAngle, intakeEncoder.getVelocity())); } public void climbingState(){ intakeRoller.set(0.0); - intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kDownAngle) + intakeFeedForward.calculate(IntakeConstants.kDownAngle, 0.0)); + intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kDownAngle) + intakeFeedForward.calculate(IntakeConstants.kDownAngle, intakeEncoder.getVelocity())); } public void intakeUp(){ intakeRoller.set(0.0); - intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kUpAngle) + intakeFeedForward.calculate(IntakeConstants.kUpAngle, 0.0)); + intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kUpAngle) + intakeFeedForward.calculate(IntakeConstants.kUpAngle, intakeEncoder.getVelocity())); } }