Add code from yesterday

src/main/java/frc4388/robot/commands/GotoLastApril.java

@@ -75,6 +75,14 @@ public class GotoLastApril extends Command {
            0
         );
 
+        Rotation2d error = new Translation2d(xerr, yerr).getAngle();
+
+        error = error.rotateBy(Rotation2d.fromDegrees(-rotoutput));
+
+        double tagRelativeXError = error.getSin();
+
+        System.out.println(tagRelativeXError);
+
         // SmartDashboard.putNumber("PID X Output", xoutput);
         // SmartDashboard.putNumber("PID Y Output", youtput);
         // // SmartDashboard.putNumber("PID Y Output", youtput);
@@ -86,6 +94,10 @@ public class GotoLastApril extends Command {
     public final boolean isFinished() {
         boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
         // System.out.println(finished);
+
+        if(finished)
+            swerveDrive.softStop();
+
         return finished;
                 // this statement is a boolean in and of itself
     }

src/main/java/frc4388/robot/commands/MoveForTimeCommand.java

@@ -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;
+    }
+}