diff --git a/src/main/java/frc/robot/commands/intake/StartOutake.java b/src/main/java/frc/robot/commands/intake/StartOutake.java new file mode 100644 index 00000000..2a32861a --- /dev/null +++ b/src/main/java/frc/robot/commands/intake/StartOutake.java @@ -0,0 +1,39 @@ +package frc.robot.commands.intake; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.Constants; +import frc.robot.subsystems.IntakeSubsystem; + +public class StartOutake extends Command { + private final IntakeSubsystem intakeSubsystem; + private final Timer timer = new Timer(); + private final int motorRunTime; + + public StartOutake(IntakeSubsystem intakeSubsystem, int motorRunTime) { + addRequirements(intakeSubsystem); + this.intakeSubsystem = intakeSubsystem; + this.motorRunTime = motorRunTime; + } + + @Override + public void initialize() { + timer.reset(); + timer.start(); + } + + @Override + public void execute() { + intakeSubsystem.setMotorSpeed(-Constants.INTAKE_MOTOR_1_SPEED, -Constants.INTAKE_MOTOR_2_SPEED); + } + + @Override + public void end(boolean interrupted) { + intakeSubsystem.stopMotors(); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(motorRunTime); + } +} \ No newline at end of file