diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 2b8b564..cf419f2 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -7,15 +7,20 @@ import com.revrobotics.CANSparkLowLevel.MotorType; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.IntakeConstants; public class Intake extends SubsystemBase{ private PIDController intakeAnglePID; + private CANSparkMax intakeRoller; private CANSparkMax intakePivot; + private RelativeEncoder intakeEncoder; + private ArmFeedforward intakeFeedForward; + public Intake(){ intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless); @@ -29,25 +34,56 @@ public class Intake extends SubsystemBase{ intakeEncoder = intakePivot.getEncoder(); intakeEncoder.setPosition(IntakeConstants.kStartingAngle); intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor); + intakeAnglePID = new PIDController(IntakeConstants.kPIntake, IntakeConstants.kIIntake, IntakeConstants.kDIntake); } - public void intakeDown(){ - intakeRoller.set(1.0); + public Command intakeDownCommand() { + return run(() -> { + intakeRoller.set(1.0); - intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kDownAngle) + intakeFeedForward.calculate(IntakeConstants.kDownAngle, intakeEncoder.getVelocity())); + intakePivot.setVoltage( + intakeAnglePID.calculate( + intakeEncoder.getPosition(), + IntakeConstants.kDownAngle + ) + intakeFeedForward.calculate( + IntakeConstants.kDownAngle, + intakeEncoder.getVelocity() + ) + ); + }); } - public void climbingState(){ - intakeRoller.set(0.0); + public Command climbingStateCommand() { + return run(() -> { + intakeRoller.set(0.0); - intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kDownAngle) + intakeFeedForward.calculate(IntakeConstants.kDownAngle, intakeEncoder.getVelocity())); + intakePivot.setVoltage( + intakeAnglePID.calculate( + intakeEncoder.getPosition(), + IntakeConstants.kDownAngle + ) + intakeFeedForward.calculate( + IntakeConstants.kDownAngle, + intakeEncoder.getVelocity() + ) + ); + }); } - public void intakeUp(){ - intakeRoller.set(0.0); + public Command intakeUpCommand() { + return run(() -> { + intakeRoller.set(0.0); - intakePivot.setVoltage(intakeAnglePID.calculate(intakeEncoder.getPosition(), IntakeConstants.kUpAngle) + intakeFeedForward.calculate(IntakeConstants.kUpAngle, intakeEncoder.getVelocity())); + intakePivot.setVoltage( + intakeAnglePID.calculate( + intakeEncoder.getPosition(), + IntakeConstants.kUpAngle + ) + intakeFeedForward.calculate( + IntakeConstants.kUpAngle, + intakeEncoder.getVelocity() + ) + ); + }); } }