From 236ee989ff3cd4a75db0784366538103b473c4b7 Mon Sep 17 00:00:00 2001 From: Tyler-J42 Date: Sat, 3 Feb 2024 02:17:15 -0500 Subject: [PATCH] feedforward calculates on current velocity --- src/main/java/frc/robot/subsystems/Intake.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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())); } }