Add code from yesterday

This commit is contained in:
Michael Mikovsky
2025-02-04 16:34:54 -07:00
parent 7fa93e6892
commit 3cacb7743e
4 changed files with 67 additions and 3 deletions
@@ -0,0 +1,47 @@
package frc4388.robot.commands;
import java.time.Instant;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;
import frc4388.robot.subsystems.SwerveDrive;
// Command to repeat a joystick movement for a specific time.
public class MoveForTimeCommand extends Command {
private final SwerveDrive swerveDrive;
private final Translation2d leftStick;
private final Translation2d rightStick;
private final long duration;
private final boolean robotRelative;
private Instant startTime;
public MoveForTimeCommand(
SwerveDrive swerveDrive,
Translation2d leftStick,
Translation2d rightStick,
long millis,
boolean robotRelative) {
this.swerveDrive = swerveDrive;
this.leftStick = leftStick;
this.rightStick = rightStick;
this.duration = millis;
this.robotRelative = robotRelative;
}
@Override
public void initialize() {
startTime = Instant.now();
}
@Override
public void execute() {
swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
}
@Override
public boolean isFinished() {
return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
}
}