diff --git a/src/main/java/frc4388/robot/commands/RunCommandForTime.java b/src/main/java/frc4388/robot/commands/RunCommandForTime.java new file mode 100644 index 0000000..0424b30 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/RunCommandForTime.java @@ -0,0 +1,78 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Subsystem; + +public class RunCommandForTime extends CommandBase { + + // command + Command command; + + // timing + long start; + long elapsed; + double duration; + + // override + boolean override; + + /** + * Runs given command for given time. + * @param command Command to run. + * @param duration Time to run for, in seconds. + * @param override If true: end command when time ends, even if the command isn't finished. If false: end command when it finished and time has ended. + */ + public RunCommandForTime(Command command, double duration, boolean override) { + // Use addRequirements() here to declare subsystem dependencies. + + this.command = command; + this.duration = duration; + this.override = override; + + addRequirements(this.command.getRequirements().toArray(Subsystem[]::new)); + } + + /** + * Runs given command for given time. + * @param command Command to run. + * @param duration Time to run for, in seconds. + */ + public RunCommandForTime(Command command, double duration) { + this(command, duration, false); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.start = System.currentTimeMillis(); + this.command.initialize(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + this.elapsed = System.currentTimeMillis() - this.start; + this.command.execute(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + this.command.end(interrupted); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (this.override) { + return (((long) this.duration) >= (this.elapsed - this.start)); + } else { + return (this.command.isFinished() && (((long) this.duration) >= (this.elapsed - this.start))); + } + } +}